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 <cob_trajectory_controller/RefValJS_PTP.h> 00019 00020 inline double sqr(double x) 00021 { 00022 return x*x; 00023 } 00024 00025 const double RefValJS_PTP::weigths[] = { 1.5, 1.5, 1.0, 1.0, 0.8, 0.8, 0.7 }; 00026 00027 00028 RefValJS_PTP::RefValJS_PTP(const std::vector<double>& start, const std::vector<double>& ziel, double v_rad_s, double a_rad_s2) 00029 { 00030 m_start = start; 00031 m_ziel = ziel; 00032 00033 m_direction.resize(start.size()); 00034 for(unsigned int i = 0; i < start.size(); i++) 00035 m_direction.at(i) = ziel.at(i) - start.at(i); 00036 double max = m_direction.at(0); 00037 double min = m_direction.at(0); 00038 for(unsigned int i = 0; i < m_direction.size(); i++) 00039 { 00040 if(max < m_direction.at(i)) 00041 max = m_direction.at(i); 00042 if(min > m_direction.at(i)) 00043 min = m_direction.at(i); 00044 } 00045 max = fabs(max); 00046 min = fabs(min); 00047 00048 //m_length = (max>min)?max:min; 00049 m_length = norm(m_direction); 00050 00051 m_v_rad_s = v_rad_s; 00052 m_a_rad_s2 = a_rad_s2; 00053 00054 /* Parameter für Beschl.begrenzte Fahrt: */ 00055 double a = fabs(m_a_rad_s2); 00056 double v = fabs(m_v_rad_s); 00057 00058 if (m_length > v*v/a) 00059 { 00060 // Phase mit konst. Geschw. wird erreicht: 00061 m_T1 = m_T3 = v / a; 00062 m_T2 = (m_length - v*v/a) / v; 00063 00064 // Wegparameter s immer positiv, deswegen keine weitere Fallunterscheidung nötig: 00065 m_sv2 = 1.0 / (m_T1 + m_T2); 00066 m_sa1 = m_sv2 / m_T1; 00067 m_sa3 = -m_sa1; 00068 } 00069 else { 00070 // Phase konst. Geschw. wird nicht erreicht: 00071 m_T2 = 0.0; 00072 m_T1 = m_T3 = sqrt(m_length / a); 00073 00074 m_sv2 = 1.0 / m_T1; 00075 m_sa1 = m_sv2 / m_T1; 00076 m_sa3 = -m_sa1; 00077 00078 } 00079 // Bewegung vollständig charakterisiert. 00080 00081 } 00082 00083 00084 std::vector<double> RefValJS_PTP::r(double s) const 00085 { 00086 std::vector<double> soll; 00087 soll.resize(m_start.size()); 00088 if (s <= 0) 00089 { 00090 soll = m_start; 00091 } else 00092 if (s < 1) 00093 { 00094 for(unsigned int i = 0; i < m_start.size(); i++) 00095 soll.at(i) = m_start.at(i) + m_direction.at(i) * s; 00096 } 00097 else 00098 { 00099 soll = m_ziel; 00100 } 00101 return soll; 00102 } 00103 00104 double RefValJS_PTP::s(double t) const 00105 { 00106 if (t >= m_T1 + m_T2 + m_T3) 00107 return 1.0; 00108 else if (t >= m_T1 + m_T2) 00109 { 00110 return 0.5*m_sa1*m_T1*m_T1 + m_sv2*m_T2 + m_sv2*(t-m_T1-m_T2) + 0.5*m_sa3*sqr(t-m_T1-m_T2); 00111 } 00112 else if (t >= m_T1) 00113 { 00114 return 0.5*m_sa1*m_T1*m_T1 + m_sv2*(t-m_T1); 00115 } 00116 else if (t > 0.0) 00117 { 00118 return 0.5*m_sa1*t*t; 00119 } 00120 else return 0.0; 00121 } 00122 00123 double RefValJS_PTP::ds_dt(double t) const 00124 { 00125 if (t >= m_T1 + m_T2 + m_T3) 00126 return 0.0; 00127 else if (t >= m_T1 + m_T2) 00128 { 00129 return m_sv2 + m_sa3*(t-m_T1-m_T2); 00130 } 00131 else if (t >= m_T1) 00132 { 00133 return m_sv2; 00134 } 00135 else if (t > 0.0) 00136 { 00137 return m_sa1*t; 00138 } 00139 else return 0.0; 00140 } 00141 00142 std::vector<double> RefValJS_PTP::dr_ds(double s) const 00143 { 00144 std::vector<double> result = m_start; 00145 if (s < 0.0 || s >= 1.0) 00146 { 00147 for(unsigned int i = 0; i<result.size(); i++) 00148 result.at(i) = 0.0; 00149 } 00150 else 00151 { 00152 result = m_direction; 00153 } 00154 00155 return result; 00156 } 00157 00158