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 #include <vector>
00019 #include <ros/ros.h>
00020 #include <cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_ramp.h>
00021
00022
00023 bool TrajectoryProfileRamp::getProfileTimings(double Se, double te, bool calcMaxTe, cob_cartesian_controller::ProfileTimings& pt)
00024 {
00025 CartesianControllerUtils utils;
00026 double tv, tb = 0.0;
00027 double vel = params_.profile.vel;
00028 double accl = params_.profile.accl;
00029
00030
00031 if (vel > sqrt(std::fabs(Se) * accl))
00032 {
00033 vel = sqrt(std::fabs(Se) * accl);
00034 }
00035
00036 if (vel > MIN_VELOCITY_THRESHOLD)
00037 {
00038 tb = utils.roundUpToMultiplier(vel / accl, params_.profile.t_ipo);
00039 if (calcMaxTe)
00040 {
00041 te = utils.roundUpToMultiplier((std::fabs(Se) / vel) + tb, params_.profile.t_ipo);
00042 }
00043 tv = te - tb;
00044
00045
00046 pt.tb = tb;
00047 pt.tv = tv;
00048 pt.te = te;
00049
00050 pt.steps_tb = pt.tb/params_.profile.t_ipo;
00051 pt.steps_tv = (pt.tv-pt.tb)/params_.profile.t_ipo;
00052 pt.steps_te = (pt.te-pt.tv)/params_.profile.t_ipo;
00053
00054 pt.vel = vel;
00055 return true;
00056 }
00057
00058 return false;
00059 }
00060
00061 std::vector<double> TrajectoryProfileRamp::getTrajectory(double se, cob_cartesian_controller::ProfileTimings pt)
00062 {
00063 std::vector<double> array;
00064 unsigned int i = 1;
00065 double direction = se/std::fabs(se);
00066 double accl = params_.profile.accl;
00067 double t_ipo = params_.profile.t_ipo;
00068
00069
00070
00071 for (; i <= pt.steps_tb; i++)
00072 {
00073 array.push_back(direction * (0.5*accl*pow((t_ipo*i), 2)));
00074 }
00075
00076 for (; i <= (pt.steps_tb + pt.steps_tv); i++)
00077 {
00078 array.push_back(direction *(pt.vel*(t_ipo*i) - 0.5*pow(pt.vel, 2)/accl));
00079 }
00080
00081 for (; i <= (pt.steps_tb + pt.steps_tv + pt.steps_te + 1); i++)
00082 {
00083 array.push_back(direction * (pt.vel * pt.tv - 0.5 * accl * pow(pt.te-(t_ipo*i), 2)));
00084 }
00085
00086 return array;
00087 }
00088