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
00035
00036
00037 #ifndef MOVEIT_TRAJECTORY_PROCESSING_ITERATIVE_PARABOLIC_SMOOTHER_
00038 #define MOVEIT_TRAJECTORY_PROCESSING_ITERATIVE_PARABOLIC_SMOOTHER_
00039
00040 #include <trajectory_msgs/JointTrajectory.h>
00041 #include <moveit_msgs/JointLimits.h>
00042 #include <moveit_msgs/RobotState.h>
00043 #include <moveit/robot_trajectory/robot_trajectory.h>
00044
00045 namespace trajectory_processing
00046 {
00047
00050 class IterativeParabolicTimeParameterization
00051 {
00052 public:
00053 IterativeParabolicTimeParameterization(unsigned int max_iterations = 100,
00054 double max_time_change_per_it = .01);
00055 ~IterativeParabolicTimeParameterization();
00056
00057 bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory) const;
00058
00059 private:
00060
00061 unsigned int max_iterations_;
00062 double max_time_change_per_it_;
00063
00064 void applyVelocityConstraints(robot_trajectory::RobotTrajectory& rob_trajectory,
00065 const std::vector<std::string>& active_joints,
00066 const std::vector<moveit_msgs::JointLimits>& limits,
00067 std::vector<double> &time_diff) const;
00068
00069 void applyAccelerationConstraints(robot_trajectory::RobotTrajectory& rob_trajectory,
00070 const std::vector<std::string>& active_joints,
00071 const std::vector<moveit_msgs::JointLimits>& limits,
00072 std::vector<double> & time_diff) const;
00073
00074 double findT1( const double d1, const double d2, double t1, const double t2, const double a_max) const;
00075 double findT2( const double d1, const double d2, const double t1, double t2, const double a_max) const;
00076 void printStats(const trajectory_msgs::JointTrajectory& trajectory,
00077 const std::vector<moveit_msgs::JointLimits>& limits) const;
00078 void printPoint(const trajectory_msgs::JointTrajectoryPoint& point, unsigned int i) const;
00079 };
00080
00081 }
00082
00083 #endif