cob_cartesian_controller: trajectory_profile_generator

Go to the documentation of this file.
00001 /*
00002  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *   http://www.apache.org/licenses/LICENSE-2.0
00009 
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 
00018 #include <vector>
00019 #include <ros/ros.h>
00020 #include <cob_cartesian_controller/trajectory_profile_generator/trajectory_profile_generator_sinoid.h>
00021 
00022 /* BEGIN TrajectoryProfileSinoid ****************************************************************************************/
00023 bool TrajectoryProfileSinoid::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     // Calculate the Sinoid Profile Parameters
00031     if (vel > sqrt(std::fabs(Se) * accl / 2))
00032     {
00033         vel = sqrt(std::fabs(Se) * accl / 2);
00034     }
00035 
00036     if (vel > MIN_VELOCITY_THRESHOLD)
00037     {
00038         tb = utils.roundUpToMultiplier(2 * vel / accl, params_.profile.t_ipo);
00039         if (te == 0)
00040         {
00041             te = utils.roundUpToMultiplier((std::fabs(Se) / vel) + tb,  params_.profile.t_ipo);
00042         }
00043         tv = te - tb;
00044 
00045         // Interpolationsteps for every timesequence
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> TrajectoryProfileSinoid::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     // Calculate the sinoid profile path
00070     // 0 <= t <= tb
00071     for (; i <= pt.steps_tb; i++)
00072     {
00073         array.push_back(direction * (accl*(0.25*pow(i*t_ipo, 2) + pow(pt.tb, 2)/(8*pow(M_PI, 2)) *(cos(2*M_PI/pt.tb * (i*t_ipo))-1))));
00074     }
00075     // tb <= t <= tv
00076     for (; i <= (pt.steps_tb + pt.steps_tv); i++)
00077     {
00078         array.push_back(direction * (pt.vel*(i*t_ipo-0.5*pt.tb)));
00079     }
00080     // tv <= t <= te
00081     for (; i <= (pt.steps_tv + pt.steps_tb + pt.steps_te + 1); i++)
00082     {
00083         array.push_back(direction * (0.5 * accl *(pt.te*(i*t_ipo + pt.tb) - 0.5*(pow(i*t_ipo, 2)+pow(pt.te, 2)+2*pow(pt.tb, 2)) + (pow(pt.tb, 2)/(4*pow(M_PI, 2))) * (1-cos(((2*M_PI)/pt.tb) * (i*t_ipo-pt.tv))))));
00084     }
00085 
00086     return array;
00087 }
00088 /* END TrajectoryProfileSinoid ******************************************************************************************/