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 <constraint_aware_spline_smoother/KunzStilman/Trajectory.h>
00038 #include <constraint_aware_spline_smoother/parabolic_linear_blend.h>
00039 #include <arm_navigation_msgs/FilterJointTrajectoryWithConstraints.h>
00040 #include <arm_navigation_msgs/JointLimits.h>
00041 #include <Eigen/Core>
00042
00043 using namespace constraint_aware_spline_smoother;
00044
00045 const double DEFAULT_VEL_MAX=1.0;
00046 const double DEFAULT_ACCEL_MAX=1.0;
00047 const double ROUNDING_THRESHOLD = 0.01;
00048 const int NUM_OUTPUT_POINTS = 100;
00049
00050
00051 template <typename T>
00052 bool ParabolicLinearBlendSmoother<T>::smooth(const T& trajectory_in,
00053 T& trajectory_out) const
00054 {
00055 std::list<Eigen::VectorXd> path;
00056 Eigen::VectorXd vmax;
00057 Eigen::VectorXd amax;
00058 double smin = 0.03;
00059
00060
00061 vmax.resize(trajectory_in.request.limits.size());
00062 amax.resize(trajectory_in.request.limits.size());
00063
00064
00065 for(unsigned int i=0; i<trajectory_in.request.limits.size(); i++)
00066 {
00067 if( trajectory_in.request.limits[i].has_velocity_limits )
00068 {
00069 vmax[i] = trajectory_in.request.limits[i].max_velocity;
00070 }
00071 else
00072 {
00073 vmax[i] = DEFAULT_VEL_MAX;
00074 }
00075 if( trajectory_in.request.limits[i].has_acceleration_limits )
00076 {
00077 amax[i] = trajectory_in.request.limits[i].max_acceleration;
00078 }
00079 else
00080 {
00081 amax[i] = DEFAULT_ACCEL_MAX;
00082 }
00083 }
00084
00085
00086
00087 for(unsigned int i=0; i<trajectory_in.request.trajectory.points.size(); i++)
00088 {
00089 Eigen::VectorXd point;
00090 point.resize(trajectory_in.request.trajectory.points[i].positions.size());
00091 for(unsigned int j=0; j<trajectory_in.request.trajectory.points[i].positions.size(); j++)
00092 {
00093 point[j]=trajectory_in.request.trajectory.points[i].positions[j];
00094 }
00095 std::vector<double> positions=trajectory_in.request.trajectory.points[i].positions;
00096 path.push_back(point);
00097 }
00098
00099 ParabolicBlend::Trajectory blend_trajectory(path,vmax,amax,smin);
00100
00101
00102 trajectory_out = trajectory_in;
00103 unsigned int num_joints = trajectory_in.request.trajectory.joint_names.size();
00104 unsigned int num_points = NUM_OUTPUT_POINTS;
00105 double duration = blend_trajectory.getDuration();
00106 double discretization = duration / num_points;
00107
00108
00109 trajectory_out.request.trajectory.points.clear();
00110 for (unsigned int i=0; i<=num_points; ++i)
00111 {
00112 double time_from_start = discretization * i;
00113 if(i==0) time_from_start=0.0;
00114
00115 Eigen::VectorXd positions = blend_trajectory.getPosition( time_from_start );
00116 Eigen::VectorXd velocities = blend_trajectory.getVelocity( time_from_start );
00117
00118 trajectory_msgs::JointTrajectoryPoint point;
00119 point.time_from_start = ros::Duration(time_from_start);
00120 point.positions.resize(num_joints);
00121 point.velocities.resize(num_joints);
00122 for (unsigned int j=0; j<num_joints; ++j)
00123 {
00124 point.positions[j] = positions[j];
00125 point.velocities[j] = velocities[j];
00126 }
00127
00128 trajectory_out.request.trajectory.points.push_back(point);
00129 }
00130
00131 return true;
00132 }
00133
00134
00135 PLUGINLIB_DECLARE_CLASS(constraint_aware_spline_smoother,
00136 ParabolicLinearBlendFilterJointTrajectoryWithConstraints,
00137 constraint_aware_spline_smoother::ParabolicLinearBlendSmoother<arm_navigation_msgs::FilterJointTrajectoryWithConstraints>,
00138 filters::FilterBase<arm_navigation_msgs::FilterJointTrajectoryWithConstraints>)