simple_trapezoidal_velocity_profile.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2 * Copyright 2018 ROBOTIS CO., LTD.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *******************************************************************************/
16 
17 /*
18  * simple_trapezoidal_velocity_profile.cpp
19  *
20  * Created on: 2016. 8. 24.
21  * Author: Jay Song
22  */
23 
24 #include <iostream>
26 
27 using namespace robotis_framework;
28 
30 {
31  pos_coeff_accel_section_.resize(3, 1);
33  vel_coeff_accel_section_.resize(3, 1);
35 
36  pos_coeff_const_section_.resize(3, 1);
38  vel_coeff_const_section_.resize(3, 1);
40 
41  pos_coeff_decel_section_.resize(3, 1);
43  vel_coeff_decel_section_.resize(3, 1);
45 
46  time_variables_.resize(1, 3);
47  time_variables_.fill(0);
48 
49  acceleration_ = 0;
50  deceleration_ = 0;
51  max_velocity_ = 0;
52 
53  initial_pos_ = 0;
54  final_pos_ = 0;
55 
56  current_time_ = 0;
57  current_pos_ = 0;
58  current_vel_ = 0;
59  current_acc_ = 0;
60 
61  accel_time_ = 0;
62  const_time_ = 0;
63  decel_time_ = 0;
64 
67  total_time_ = 0;
68 }
69 
71 {
72 
73 }
74 
75 void SimpleTrapezoidalVelocityProfile::setVelocityBaseTrajectory(double init_pos, double final_pos, double acceleration, double max_velocity)
76 {
77  setVelocityBaseTrajectory(init_pos, final_pos, acceleration, acceleration, max_velocity);
78 }
79 
80 void SimpleTrapezoidalVelocityProfile::setVelocityBaseTrajectory(double init_pos, double final_pos, double acceleration, double deceleration, double max_velocity)
81 {
82  double pos_diff = final_pos - init_pos;
83  double acc_time = fabs(max_velocity / acceleration);
84  double dec_time = fabs(max_velocity / deceleration);
85 
86  acceleration_ = copysign(fabs(acceleration), pos_diff);
87  deceleration_ = copysign(fabs(deceleration), -pos_diff);
88  max_velocity_ = copysign(fabs(max_velocity), pos_diff);
89 
90  initial_pos_ = init_pos;
91  final_pos_ = final_pos;
92 
93  if(fabs(pos_diff) > 0.5 * fabs(max_velocity_) * ( acc_time + dec_time ))
94  {
95  accel_time_ = acc_time;
96  decel_time_ = dec_time;
97 
98  const_time_ = (fabs(pos_diff) - 0.5 * fabs(max_velocity_) * ( acc_time + dec_time )) / fabs(max_velocity_);
102  }
103  else
104  {
105  accel_time_ = sqrt(2*fabs(pos_diff)*fabs(deceleration_) / (acceleration_*acceleration_ + fabs(acceleration_*deceleration_)) );
106  decel_time_ = accel_time_*fabs(acceleration_/deceleration_);
107 
108  const_time_ = 0;
112  }
113 
114  //acc section
115  pos_coeff_accel_section_.coeffRef(0,0) = 0.5*acceleration_;
116  pos_coeff_accel_section_.coeffRef(1,0) = 0;
117  pos_coeff_accel_section_.coeffRef(2,0) = initial_pos_;
118 
119  vel_coeff_accel_section_.coeffRef(0,0) = 0;
120  vel_coeff_accel_section_.coeffRef(1,0) = acceleration_;
121  vel_coeff_accel_section_.coeffRef(2,0) = 0;
122 
123  //const section
124  pos_coeff_const_section_.coeffRef(0,0) = 0;
125  pos_coeff_const_section_.coeffRef(1,0) = max_velocity_;
127 
128  vel_coeff_const_section_.coeffRef(0,0) = 0;
129  vel_coeff_const_section_.coeffRef(1,0) = 0;
130  vel_coeff_const_section_.coeffRef(2,0) = max_velocity_;
131 
132  //decel section
133  pos_coeff_decel_section_.coeffRef(0,0) = 0.5*deceleration_;
135  pos_coeff_decel_section_.coeffRef(2,0) = 0.5*deceleration_*total_time_*total_time_ + final_pos_;
136 
137  vel_coeff_decel_section_.coeffRef(0,0) = 0;
138  vel_coeff_decel_section_.coeffRef(1,0) = deceleration_;
140 }
141 
142 void SimpleTrapezoidalVelocityProfile::setTimeBaseTrajectory(double init_pos, double final_pos, double accel_time, double total_time)
143 {
144  setTimeBaseTrajectory(init_pos, final_pos, accel_time, accel_time, total_time);
145 }
146 
147 void SimpleTrapezoidalVelocityProfile::setTimeBaseTrajectory(double init_pos, double final_pos, double accel_time, double decel_time, double total_time)
148 {
149  initial_pos_ = init_pos;
150  final_pos_ = final_pos;
151  total_time_ = fabs(total_time);
152 
153  if((fabs(accel_time) + fabs(decel_time)) <= total_time_)
154  {
155  accel_time_ = fabs(accel_time);
156  decel_time_ = fabs(decel_time);
157  const_time_ = total_time - accel_time_ - decel_time_;
158  }
159  else
160  {
161  double time_gain = total_time_ / (fabs(accel_time) + fabs(decel_time));
162  accel_time_ = time_gain*fabs(accel_time);
163  decel_time_ = time_gain*fabs(decel_time);
164  const_time_ = 0;
165  }
166 
169 
170  double pos_diff = final_pos - init_pos;
171  max_velocity_ = 2*pos_diff / (total_time_ + const_time_);
174 
175  //acc section
176  pos_coeff_accel_section_.coeffRef(0,0) = 0.5*acceleration_;
177  pos_coeff_accel_section_.coeffRef(1,0) = 0;
178  pos_coeff_accel_section_.coeffRef(2,0) = initial_pos_;
179 
180  vel_coeff_accel_section_.coeffRef(0,0) = 0;
181  vel_coeff_accel_section_.coeffRef(1,0) = acceleration_;
182  vel_coeff_accel_section_.coeffRef(2,0) = 0;
183 
184  //const section
185  pos_coeff_const_section_.coeffRef(0,0) = 0;
186  pos_coeff_const_section_.coeffRef(1,0) = max_velocity_;
188 
189  vel_coeff_const_section_.coeffRef(0,0) = 0;
190  vel_coeff_const_section_.coeffRef(1,0) = 0;
191  vel_coeff_const_section_.coeffRef(2,0) = max_velocity_;
192 
193  //decel section
194  pos_coeff_decel_section_.coeffRef(0,0) = 0.5*deceleration_;
196  pos_coeff_decel_section_.coeffRef(2,0) = 0.5*deceleration_*total_time_*total_time_ + final_pos_;
197 
198  vel_coeff_decel_section_.coeffRef(0,0) = 0;
199  vel_coeff_decel_section_.coeffRef(1,0) = deceleration_;
201 }
202 
204 {
205  setTime(time);
206  return current_pos_;
207 }
208 
210 {
211  setTime(time);
212  return current_vel_;
213 }
214 
216 {
217  setTime(time);
218  return current_acc_;
219 }
220 
222 {
223  time_variables_ << powDI(time, 2), time, 1.0;
224 
225  if(time <= 0)
226  {
227  current_time_ = 0;
229  current_vel_ = 0;
230  current_acc_ = 0;
231  }
232  else if(time < const_start_time_)
233  {
234  current_time_ = time;
238  }
239  else if(time <= decel_start_time_)
240  {
241  current_time_ = time;
244  current_acc_ = 0;
245  }
246  else if(time < total_time_)
247  {
248  current_time_ = time;
252  }
253  else
254  {
257  current_vel_ = 0;
258  current_acc_ = 0;
259  }
260 }
261 
263 {
264  return current_pos_;
265 }
266 
268 {
269  return current_vel_;
270 }
271 
273 {
274  return current_acc_;
275 }
276 
278 {
279  return total_time_;
280 }
281 
283 {
284  return const_start_time_;
285 }
286 
288 {
289  return decel_start_time_;
290 }
291 
292 
void setTimeBaseTrajectory(double init_pos, double final_pos, double accel_time, double total_time)
void setVelocityBaseTrajectory(double init_pos, double final_pos, double acceleration, double max_velocity)
double powDI(double a, int b)


robotis_math
Author(s): SCH , Kayman , Jay Song
autogenerated on Fri Jul 17 2020 03:17:50