minimum_jerk_trajectory_with_via_point.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 
18 
19 using namespace robotis_framework;
20 
21 MinimumJerkViaPoint::MinimumJerkViaPoint(double ini_time, double fin_time, double via_time, double ratio,
22  std::vector<double_t> ini_pos, std::vector<double_t> ini_vel, std::vector<double_t> ini_acc,
23  std::vector<double_t> fin_pos, std::vector<double_t> fin_vel, std::vector<double_t> fin_acc,
24  std::vector<double_t> via_pos, std::vector<double_t> via_vel, std::vector<double_t> via_acc)
25 {
26  ratio_ = ratio;
27 
28 // input_ini_time_ = ini_time;
29 // input_fin_time_ = fin_time;
30  via_time_ = via_time;
31 
32  double time_size = fin_time - ini_time;
33  double time_ratio = 0.5*ratio_*time_size;
34 
35  ini_time_ = ini_time + time_ratio;
36  fin_time_ = fin_time - time_ratio;
37 
38  ini_pos_ = ini_pos;
39  ini_vel_ = ini_vel;
40  ini_acc_ = ini_acc;
41  fin_pos_ = fin_pos;
42  fin_vel_ = fin_vel;
43  fin_acc_ = fin_acc;
44  via_pos_ = via_pos;
45 // via_vel_ = via_vel;
46 // via_acc_ = via_acc;
47 
48  number_of_joint_ = ini_pos.size();
49 
53  time_variables_.resize(1,7);
54 
55  position_coeff_.fill(0.0);
56  velocity_coeff_.fill(0.0);
57  acceleration_coeff_.fill(0.0);
58  time_variables_.fill(0.0);
59 
60  if(fin_time_ > ini_time_)
61  {
62  Eigen::MatrixXd time_mat;
63  Eigen::MatrixXd conditions_mat;
64 
65  time_mat.resize(7,7);
66  time_mat << powDI(ini_time_,5), powDI(ini_time_,4), powDI(ini_time_,3), powDI(ini_time_,2), ini_time_, 1.0, 0.0,
67  5.0*powDI(ini_time_,4), 4.0*powDI(ini_time_,3), 3.0*powDI(ini_time_,2), 2.0*ini_time_, 1.0, 0.0, 0.0,
68  20.0*powDI(ini_time_,3), 12.0*powDI(ini_time_,2), 6.0*ini_time_, 2.0, 0.0, 0.0, 0.0,
70  5.0*powDI(fin_time_,4), 4.0*powDI(fin_time_,3), 3.0*powDI(fin_time_,2), 2.0*fin_time_, 1.0, 0.0, powDI(fin_time_ - via_time_,4)/24.0,
71  20.0*powDI(fin_time_,3), 12.0*powDI(fin_time_,2), 6.0*fin_time_, 2.0, 0.0, 0.0, powDI(fin_time_ - via_time_,3)/6.0,
73 
74  conditions_mat.resize(7,1);
75  conditions_mat.fill(0.0);
76 
77  for (int i = 0; i < number_of_joint_; i++)
78  {
79  conditions_mat.coeffRef(0,0) = ini_pos[i];
80  conditions_mat.coeffRef(1,0) = ini_vel[i];
81  conditions_mat.coeffRef(2,0) = ini_acc[i];
82  conditions_mat.coeffRef(3,0) = fin_pos[i];
83  conditions_mat.coeffRef(4,0) = fin_vel[i];
84  conditions_mat.coeffRef(5,0) = fin_acc[i];
85  conditions_mat.coeffRef(6,0) = via_pos[i];
86 
87  Eigen::MatrixXd position_coeff = time_mat.inverse() * conditions_mat;
88 
89  position_coeff_.block(0,i,7,1) = position_coeff;
90 
91  velocity_coeff_.coeffRef(0,i) = 0.0;
92  velocity_coeff_.coeffRef(1,i) = 5.0*position_coeff.coeff(0,0);
93  velocity_coeff_.coeffRef(2,i) = 4.0*position_coeff.coeff(1,0);
94  velocity_coeff_.coeffRef(3,i) = 3.0*position_coeff.coeff(2,0);
95  velocity_coeff_.coeffRef(4,i) = 2.0*position_coeff.coeff(3,0);
96  velocity_coeff_.coeffRef(5,i) = position_coeff.coeff(4,0);
97  velocity_coeff_.coeffRef(6,i) = position_coeff.coeff(6,0);
98 
99  acceleration_coeff_.coeffRef(0,i) = 0.0;
100  acceleration_coeff_.coeffRef(1,i) = 0.0;
101  acceleration_coeff_.coeffRef(2,i) = 20.0*position_coeff_.coeff(0,0);
102  acceleration_coeff_.coeffRef(3,i) = 12.0*position_coeff_.coeff(1,0);
103  acceleration_coeff_.coeffRef(4,i) = 6.0*position_coeff_.coeff(2,0);
104  acceleration_coeff_.coeffRef(5,i) = 2.0*position_coeff_.coeff(3,0);
105  acceleration_coeff_.coeffRef(5,i) = position_coeff_.coeff(6,0);
106  }
107  }
108 
109  cur_time_ = 0.0;
110  cur_pos_.resize(number_of_joint_);
111  cur_vel_.resize(number_of_joint_);
112  cur_acc_.resize(number_of_joint_);
113 }
114 
116 {
117 
118 }
119 
120 std::vector<double_t> MinimumJerkViaPoint::getPosition(double time)
121 {
122  if(time >= fin_time_)
123  cur_pos_ = fin_pos_;
124  else if(time <= ini_time_)
125  cur_pos_ = ini_pos_;
126  else if (time > ini_time_ && time <= via_time_)
127  {
128  cur_time_ = time;
129  time_variables_ << powDI(time, 5), powDI(time, 4), powDI(time, 3), powDI(time, 2), time, 1.0, 0.0;
130 
131  for (int i = 0; i < number_of_joint_; i++)
132  {
133  Eigen::MatrixXd cur_pos = time_variables_ * position_coeff_.block(0,i,7,1);
134  cur_pos_[i] = cur_pos.coeff(0,0);
135  }
136  }
137  else if (time < fin_time_ && time > via_time_)
138  {
139  cur_time_ = time;
140  time_variables_ << powDI(time, 5), powDI(time, 4), powDI(time, 3), powDI(time, 2), time, 1.0, powDI(time - via_time_,5)/120.0;
141 
142  for (int i = 0; i < number_of_joint_; i++)
143  {
144  Eigen::MatrixXd cur_pos = time_variables_ * position_coeff_.block(0,i,7,1);
145  cur_pos_[i] = cur_pos.coeff(0,0);
146  }
147  }
148  return cur_pos_;
149 }
150 
151 std::vector<double_t> MinimumJerkViaPoint::getVelocity(double time)
152 {
153  if(time >= fin_time_)
154  cur_vel_ = fin_vel_;
155  else if(time <= ini_time_)
156  cur_vel_ = ini_vel_;
157  else if (time > ini_time_ && time <= via_time_)
158  {
159  cur_time_ = time;
160  time_variables_ << powDI(time, 5), powDI(time, 4), powDI(time, 3), powDI(time, 2), time, 1.0, 0.0;
161 
162  for (int i = 0; i < number_of_joint_; i++)
163  {
164  Eigen::MatrixXd cur_vel = time_variables_ * velocity_coeff_.block(0,i,7,1);
165  cur_vel_[i] = cur_vel.coeff(0,0);
166  }
167  }
168  else if (time < fin_time_ && time > via_time_)
169  {
170  cur_time_ = time;
171  time_variables_ << powDI(time, 5), powDI(time, 4), powDI(time, 3), powDI(time, 2), time, 1.0, powDI(time - via_time_,5)/120.0;
172 
173  for (int i = 0; i < number_of_joint_; i++)
174  {
175  Eigen::MatrixXd cur_vel = time_variables_ * velocity_coeff_.block(0,i,7,1);
176  cur_vel_[i] = cur_vel.coeff(0,0);
177  }
178  }
179  return cur_vel_;
180 }
181 
182 std::vector<double_t> MinimumJerkViaPoint::getAcceleration(double time)
183 {
184  if(time >= fin_time_)
185  cur_acc_ = fin_acc_;
186  else if(time <= ini_time_)
187  cur_acc_ = ini_acc_;
188  else if (time > ini_time_ && time <= via_time_)
189  {
190  cur_time_ = time;
191  time_variables_ << powDI(time, 5), powDI(time, 4), powDI(time, 3), powDI(time, 2), time, 1.0, 0.0;
192 
193  for (int i = 0; i < number_of_joint_; i++)
194  {
195  Eigen::MatrixXd cur_acc = time_variables_ * acceleration_coeff_.block(0,i,7,1);
196  cur_acc_[i] = cur_acc.coeff(0,0);
197  }
198  }
199  else if (time < fin_time_ && time > via_time_)
200  {
201  cur_time_ = time;
202  time_variables_ << powDI(time, 5), powDI(time, 4), powDI(time, 3), powDI(time, 2), time, 1.0, powDI(time - via_time_,5)/120.0;
203 
204  for (int i = 0; i < number_of_joint_; i++)
205  {
206  Eigen::MatrixXd cur_acc = time_variables_ * acceleration_coeff_.block(0,i,7,1);
207  cur_acc_[i] = cur_acc.coeff(0,0);
208  }
209  }
210 
211  return cur_acc_;
212 }
MinimumJerkViaPoint(double ini_time, double fin_time, double via_time, double ratio, std::vector< double_t > ini_pos, std::vector< double_t > ini_vel, std::vector< double_t > ini_acc, std::vector< double_t > fin_pos, std::vector< double_t > fin_vel, std::vector< double_t > fin_acc, std::vector< double_t > via_pos, std::vector< double_t > via_vel, std::vector< double_t > via_acc)
double powDI(double a, int b)


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