custom_trajectory.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 /* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */
18 
19 #include "../include/open_manipulator_libs/custom_trajectory.h"
20 
21 using namespace custom_trajectory;
22 using namespace Eigen;
23 
24 
25 /*****************************************************************************
26 ** Line
27 *****************************************************************************/
28 void Line::initLine(double move_time, TaskWaypoint start, TaskWaypoint delta)
29 {
30  move_time_ = move_time;
31  acc_dec_time_ = move_time_ * 0.2;
32  vel_max_.resize(3);
33 
34  TaskWaypoint start_to_goal;
35 
36  start_pose_ = start;
37 
38  goal_pose_ .kinematic.orientation = start_pose_.kinematic.orientation;
39  goal_pose_ .kinematic.position = start.kinematic.position + delta.kinematic.position;
40 
41  vel_max_.at(X_AXIS) = delta.kinematic.position(X_AXIS)/(move_time_ - acc_dec_time_);
42  vel_max_.at(Y_AXIS) = delta.kinematic.position(Y_AXIS)/(move_time_ - acc_dec_time_);
43  vel_max_.at(Z_AXIS) = delta.kinematic.position(Z_AXIS)/(move_time_ - acc_dec_time_);
44 }
45 
46 TaskWaypoint Line::drawLine(double time_var)
47 {
48  TaskWaypoint pose;
49 
50  if(acc_dec_time_ >= time_var)
51  {
52  pose.kinematic.position(X_AXIS) = 0.5*vel_max_.at(X_AXIS)*pow(time_var, 2)/acc_dec_time_ + start_pose_.kinematic.position(X_AXIS);
53  pose.kinematic.position(Y_AXIS) = 0.5*vel_max_.at(Y_AXIS)*pow(time_var, 2)/acc_dec_time_ + start_pose_.kinematic.position(Y_AXIS);
54  pose.kinematic.position(Z_AXIS) = 0.5*vel_max_.at(Z_AXIS)*pow(time_var, 2)/acc_dec_time_ + start_pose_.kinematic.position(Z_AXIS);
55  }
56  else if(time_var > acc_dec_time_ && (move_time_ - acc_dec_time_) >= time_var )
57  {
58  pose.kinematic.position(X_AXIS) = vel_max_.at(X_AXIS)*(time_var-(acc_dec_time_*0.5)) + start_pose_.kinematic.position(X_AXIS);
59  pose.kinematic.position(Y_AXIS) = vel_max_.at(Y_AXIS)*(time_var-(acc_dec_time_*0.5)) + start_pose_.kinematic.position(Y_AXIS);
60  pose.kinematic.position(Z_AXIS) = vel_max_.at(Z_AXIS)*(time_var-(acc_dec_time_*0.5)) + start_pose_.kinematic.position(Z_AXIS);
61  }
62  else if(time_var > (move_time_ - acc_dec_time_) && (time_var < move_time_))
63  {
64  pose.kinematic.position(X_AXIS) = goal_pose_.kinematic.position(X_AXIS) - vel_max_.at(X_AXIS)*0.5/acc_dec_time_*(pow((move_time_-time_var),2));
65  pose.kinematic.position(Y_AXIS) = goal_pose_.kinematic.position(Y_AXIS) - vel_max_.at(Y_AXIS)*0.5/acc_dec_time_*(pow((move_time_-time_var),2));
66  pose.kinematic.position(Z_AXIS) = goal_pose_.kinematic.position(Z_AXIS) - vel_max_.at(Z_AXIS)*0.5/acc_dec_time_*(pow((move_time_-time_var),2));
67  }
68  else if(time_var <= move_time_)
69  {
70  pose.kinematic.position(X_AXIS) = goal_pose_.kinematic.position(X_AXIS);
71  pose.kinematic.position(Y_AXIS) = goal_pose_.kinematic.position(Y_AXIS);
72  pose.kinematic.position(Z_AXIS) = goal_pose_.kinematic.position(Z_AXIS);
73  }
74 
75  pose.kinematic.orientation = start_pose_.kinematic.orientation;
76  pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3);
77  pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3);
78  pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3);
79  pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3);
80 
81  return pose;
82 }
83 
84 TaskWaypoint Line::getTaskWaypoint(double tick)
85 {
86  return drawLine(tick);
87 }
88 
89 
90 void Line::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg)
91 {
92  TaskWaypoint *c_arg = (TaskWaypoint *)arg;
93  initLine(move_time, start, c_arg[0]);
94 }
95 void Line::setOption(const void *arg) {}
96 
97 
98 /*****************************************************************************
99 ** Circle
100 *****************************************************************************/
101 void Circle::initCircle(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position)
102 {
103  start_pose_ = start;
104 
105  radius_ = radius;
106  revolution_ = revolution;
107  start_angular_position_ = start_angular_position;
108 
109  Point drawingStart, drawingGoal;
110 
111  drawingStart.position = 0.0;
112  drawingStart.velocity = 0.0;
113  drawingStart.acceleration = 0.0;
114  drawingStart.effort = 0.0;
115 
116  drawingGoal.position = revolution_ * 2*M_PI;
117  drawingGoal.velocity = 0.0;
118  drawingGoal.acceleration = 0.0;
119  drawingGoal.effort = 0.0;
120 
121  path_generator_.calcCoefficient(drawingStart, drawingGoal, move_time);
122  coefficient_ = path_generator_.getCoefficient();
123 }
124 
125 TaskWaypoint Circle::drawCircle(double tick)
126 {
127  // get time variable
128  double get_time_var = 0.0;
129 
130  get_time_var = coefficient_(0) +
131  coefficient_(1) * pow(tick, 1) +
132  coefficient_(2) * pow(tick, 2) +
133  coefficient_(3) * pow(tick, 3) +
134  coefficient_(4) * pow(tick, 4) +
135  coefficient_(5) * pow(tick, 5);
136 
137  // set drawing trajectory
138  TaskWaypoint pose;
139 
140  double diff_pose[2];
141 
142  diff_pose[0] = (cos(get_time_var)-1)*cos(start_angular_position_) - sin(get_time_var)*sin(start_angular_position_);
143  diff_pose[1] = (cos(get_time_var)-1)*sin(start_angular_position_) + sin(get_time_var)*cos(start_angular_position_);
144 
145  pose.kinematic.position(X_AXIS) = start_pose_.kinematic.position(X_AXIS) + radius_ * diff_pose[0];
146  pose.kinematic.position(Y_AXIS) = start_pose_.kinematic.position(Y_AXIS) + radius_ * diff_pose[1];
147  pose.kinematic.position(Z_AXIS) = start_pose_.kinematic.position(Z_AXIS);
148 
149  pose.kinematic.orientation = start_pose_.kinematic.orientation;
150 
151  pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3);
152  pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3);
153  pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3);
154  pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3);
155 
156  return pose;
157 }
158 
159 TaskWaypoint Circle::getTaskWaypoint(double tick)
160 {
161  return drawCircle(tick);
162 }
163 
164 void Circle::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg)
165 {
166  double *get_arg_ = (double *)arg;
167  initCircle(move_time, start, get_arg_[0], get_arg_[1], get_arg_[2]);
168 }
169 
170 void Circle::setOption(const void *arg){}
171 
172 
173 /*****************************************************************************
174 ** Rhombus
175 *****************************************************************************/
176 void Rhombus::initRhombus(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position)
177 {
178  start_pose_ = start;
179 
180  radius_ = radius;
181  revolution_ = revolution;
182  start_angular_position_ = start_angular_position;
183 
184  Point drawingStart, drawingGoal;
185 
186  drawingStart.position = 0.0;
187  drawingStart.velocity = 0.0;
188  drawingStart.acceleration = 0.0;
189  drawingStart.effort = 0.0;
190 
191  drawingGoal.position = revolution_ * 2*M_PI;
192  drawingGoal.velocity = 0.0;
193  drawingGoal.acceleration = 0.0;
194  drawingGoal.effort = 0.0;
195 
196  path_generator_.calcCoefficient(drawingStart, drawingGoal, move_time);
197  coefficient_ = path_generator_.getCoefficient();
198 }
199 
200 
201 TaskWaypoint Rhombus::drawRhombus(double tick)
202 {
203  // get time variable
204  double get_time_var = 0.0;
205 
206  get_time_var = coefficient_(0) +
207  coefficient_(1) * pow(tick, 1) +
208  coefficient_(2) * pow(tick, 2) +
209  coefficient_(3) * pow(tick, 3) +
210  coefficient_(4) * pow(tick, 4) +
211  coefficient_(5) * pow(tick, 5);
212 
213  // set drawing trajectory
214  TaskWaypoint pose;
215  double diff_pose[2];
216  double traj[2];
217 
218  while(true)
219  {
220  if (get_time_var < PI*2) break;
221  get_time_var = get_time_var - PI*2;
222  }
223 
224  if (get_time_var >= 0 && get_time_var < PI/2){
225  traj[0] = - get_time_var / (PI/2);
226  traj[1] = - get_time_var / (PI/2);
227  } else if (get_time_var >= PI/2 && get_time_var < PI){
228  traj[0] = - get_time_var / (PI/2);
229  traj[1] = get_time_var / (PI/2) - 2;
230  } else if (get_time_var >= PI && get_time_var < PI*3/2){
231  traj[0] = get_time_var / (PI/2) - 4;
232  traj[1] = get_time_var / (PI/2) - 2;
233  } else {
234  traj[0] = get_time_var / (PI/2) - 4;
235  traj[1] = - get_time_var / (PI/2) + 4;
236  }
237 
238 
239  diff_pose[0] = traj[0]*cos(start_angular_position_) - traj[1]*sin(start_angular_position_);
240  diff_pose[1] = traj[0]*sin(start_angular_position_) + traj[1]*cos(start_angular_position_);
241 
242  pose.kinematic.position(X_AXIS) = start_pose_.kinematic.position(X_AXIS) + radius_ * diff_pose[0];
243  pose.kinematic.position(Y_AXIS) = start_pose_.kinematic.position(Y_AXIS) + radius_ * diff_pose[1];
244  pose.kinematic.position(Z_AXIS) = start_pose_.kinematic.position(Z_AXIS);
245 
246  pose.kinematic.orientation = start_pose_.kinematic.orientation;
247 
248  pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3);
249  pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3);
250  pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3);
251  pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3);
252 
253  return pose;
254 }
255 
256 
257 void Rhombus::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg)
258 {
259  double *get_arg_ = (double *)arg;
260  initRhombus(move_time, start, get_arg_[0], get_arg_[1], get_arg_[2]);
261 }
262 
263 TaskWaypoint Rhombus::getTaskWaypoint(double tick)
264 {
265  return drawRhombus(tick);
266 }
267 void Rhombus::setOption(const void *arg){}
268 
269 
270 /*****************************************************************************
271 ** Heart
272 *****************************************************************************/
273 void Heart::initHeart(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position)
274 {
275  start_pose_ = start;
276 
277  radius_ = radius;
278  revolution_ = revolution;
279  start_angular_position_ = start_angular_position;
280 
281  Point drawingStart, drawingGoal;
282 
283  drawingStart.position = 0.0;
284  drawingStart.velocity = 0.0;
285  drawingStart.acceleration = 0.0;
286  drawingStart.effort = 0.0;
287 
288  drawingGoal.position = revolution_ * 2*M_PI;
289  drawingGoal.velocity = 0.0;
290  drawingGoal.acceleration = 0.0;
291  drawingGoal.effort = 0.0;
292 
293  path_generator_.calcCoefficient(drawingStart, drawingGoal, move_time);
294  coefficient_ = path_generator_.getCoefficient();
295 }
296 
297 TaskWaypoint Heart::drawHeart(double tick)
298 {
299  // get time variable
300  double get_time_var = 0.0;
301 
302  get_time_var = coefficient_(0) +
303  coefficient_(1) * pow(tick, 1) +
304  coefficient_(2) * pow(tick, 2) +
305  coefficient_(3) * pow(tick, 3) +
306  coefficient_(4) * pow(tick, 4) +
307  coefficient_(5) * pow(tick, 5);
308 
309  // set drawing trajectory
310  TaskWaypoint pose;
311  double diff_pose[2];
312  double traj[2];
313 
314  double shift_offset = - 5.0;
315 
316  traj[0] = (shift_offset + (13*cos(get_time_var) - 5*cos(2*get_time_var) - 2*cos(3*get_time_var) - cos(4*get_time_var))) / 16;
317  traj[1] = (16*sin(get_time_var)*sin(get_time_var)*sin(get_time_var)) / 16;
318 
319  diff_pose[0] = traj[0]*cos(start_angular_position_) - traj[1]*sin(start_angular_position_);
320  diff_pose[1] = traj[0]*sin(start_angular_position_) + traj[1]*cos(start_angular_position_);
321 
322  pose.kinematic.position(X_AXIS) = start_pose_.kinematic.position(X_AXIS) + radius_ * diff_pose[0];
323  pose.kinematic.position(Y_AXIS) = start_pose_.kinematic.position(Y_AXIS) + radius_ * diff_pose[1];
324  pose.kinematic.position(Z_AXIS) = start_pose_.kinematic.position(Z_AXIS);
325 
326  pose.kinematic.orientation = start_pose_.kinematic.orientation;
327 
328  pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3);
329  pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3);
330  pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3);
331  pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3);
332 
333  return pose;
334 }
335 
336 void Heart::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg)
337 {
338  double *get_arg_ = (double *)arg;
339  initHeart(move_time, start, get_arg_[0], get_arg_[1], get_arg_[2]);
340 }
341 void Heart::setOption(const void *arg){}
342 
343 TaskWaypoint Heart::getTaskWaypoint(double tick)
344 {
345  return drawHeart(tick);
346 }
robotis_manipulator::_Point::acceleration
double acceleration
custom_trajectory::Circle::setOption
virtual void setOption(const void *arg)
Definition: custom_trajectory.cpp:170
robotis_manipulator::_Point::effort
double effort
custom_trajectory::Heart::makeTaskTrajectory
virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg)
Definition: custom_trajectory.cpp:336
custom_trajectory::Line::drawLine
TaskWaypoint drawLine(double time_var)
Definition: custom_trajectory.cpp:46
custom_trajectory::Rhombus::initRhombus
void initRhombus(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position)
Definition: custom_trajectory.cpp:176
robotis_manipulator::_DynamicPose::angular
Dynamicvector angular
robotis_manipulator::_Point::velocity
double velocity
custom_trajectory::X_AXIS
Definition: custom_trajectory.h:35
custom_trajectory::Rhombus::drawRhombus
TaskWaypoint drawRhombus(double time_var)
Definition: custom_trajectory.cpp:201
custom_trajectory::Line::setOption
virtual void setOption(const void *arg)
Definition: custom_trajectory.cpp:95
custom_trajectory::Line::makeTaskTrajectory
virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg)
Definition: custom_trajectory.cpp:90
robotis_manipulator::_TaskWaypoint::kinematic
KinematicPose kinematic
custom_trajectory::Heart::setOption
virtual void setOption(const void *arg)
Definition: custom_trajectory.cpp:341
custom_trajectory::Circle::drawCircle
TaskWaypoint drawCircle(double time_var)
Definition: custom_trajectory.cpp:125
robotis_manipulator::_Dynamicvector::velocity
Eigen::Vector3d velocity
custom_trajectory::Heart::initHeart
void initHeart(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position)
Definition: custom_trajectory.cpp:273
custom_trajectory
Definition: custom_trajectory.h:31
custom_trajectory::Circle::getTaskWaypoint
virtual TaskWaypoint getTaskWaypoint(double tick)
Definition: custom_trajectory.cpp:159
custom_trajectory::Rhombus::setOption
virtual void setOption(const void *arg)
Definition: custom_trajectory.cpp:267
custom_trajectory::Circle::initCircle
void initCircle(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position)
Definition: custom_trajectory.cpp:101
custom_trajectory::Circle::makeTaskTrajectory
virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg)
Definition: custom_trajectory.cpp:164
custom_trajectory::Y_AXIS
Definition: custom_trajectory.h:36
robotis_manipulator::_KinematicPose::position
Eigen::Vector3d position
robotis_manipulator::_TaskWaypoint::dynamic
DynamicPose dynamic
custom_trajectory::Z_AXIS
Definition: custom_trajectory.h:37
custom_trajectory::Line::getTaskWaypoint
virtual TaskWaypoint getTaskWaypoint(double tick)
Definition: custom_trajectory.cpp:84
robotis_manipulator::_DynamicPose::linear
Dynamicvector linear
custom_trajectory::Heart::getTaskWaypoint
virtual TaskWaypoint getTaskWaypoint(double tick)
Definition: custom_trajectory.cpp:343
robotis_manipulator::_Point
robotis_manipulator::_Dynamicvector::acceleration
Eigen::Vector3d acceleration
custom_trajectory::Rhombus::getTaskWaypoint
virtual TaskWaypoint getTaskWaypoint(double tick)
Definition: custom_trajectory.cpp:263
robotis_manipulator::_TaskWaypoint
PI
#define PI
robotis_manipulator::_KinematicPose::orientation
Eigen::Matrix3d orientation
robotis_manipulator::_Point::position
double position
custom_trajectory::Rhombus::makeTaskTrajectory
virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg)
Definition: custom_trajectory.cpp:257
custom_trajectory::Heart::drawHeart
TaskWaypoint drawHeart(double tick)
Definition: custom_trajectory.cpp:297
custom_trajectory::Line::initLine
void initLine(double move_time, TaskWaypoint start, TaskWaypoint delta)
Definition: custom_trajectory.cpp:28

open_manipulator_libs
Author(s): Darby Lim , Hye-Jong KIM , Ryan Shim , Yong-Ho Na
autogenerated on Mon Jun 10 2019 14:12:00