19 #include "../include/open_manipulator_libs/custom_trajectory.h" 22 using namespace Eigen;
30 move_time_ = move_time;
31 acc_dec_time_ = move_time_ * 0.2;
50 if(acc_dec_time_ >= time_var)
56 else if(time_var > acc_dec_time_ && (move_time_ - acc_dec_time_) >= time_var )
62 else if(time_var > (move_time_ - acc_dec_time_) && (time_var < move_time_))
68 else if(time_var <= move_time_)
86 return drawLine(tick);
93 initLine(move_time, start, c_arg[0]);
106 revolution_ = revolution;
107 start_angular_position_ = start_angular_position;
109 Point drawingStart, drawingGoal;
114 drawingStart.
effort = 0.0;
116 drawingGoal.
position = revolution_ * 2*M_PI;
121 path_generator_.calcCoefficient(drawingStart, drawingGoal, move_time);
122 coefficient_ = path_generator_.getCoefficient();
128 double get_time_var = 0.0;
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);
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_);
161 return drawCircle(tick);
166 double *get_arg_ = (
double *)arg;
167 initCircle(move_time, start, get_arg_[0], get_arg_[1], get_arg_[2]);
181 revolution_ = revolution;
182 start_angular_position_ = start_angular_position;
184 Point drawingStart, drawingGoal;
189 drawingStart.
effort = 0.0;
191 drawingGoal.
position = revolution_ * 2*M_PI;
196 path_generator_.calcCoefficient(drawingStart, drawingGoal, move_time);
197 coefficient_ = path_generator_.getCoefficient();
204 double get_time_var = 0.0;
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);
220 if (get_time_var <
PI*2)
break;
221 get_time_var = get_time_var -
PI*2;
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;
234 traj[0] = get_time_var / (
PI/2) - 4;
235 traj[1] = - get_time_var / (
PI/2) + 4;
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_);
259 double *get_arg_ = (
double *)arg;
260 initRhombus(move_time, start, get_arg_[0], get_arg_[1], get_arg_[2]);
265 return drawRhombus(tick);
278 revolution_ = revolution;
279 start_angular_position_ = start_angular_position;
281 Point drawingStart, drawingGoal;
286 drawingStart.
effort = 0.0;
288 drawingGoal.
position = revolution_ * 2*M_PI;
293 path_generator_.calcCoefficient(drawingStart, drawingGoal, move_time);
294 coefficient_ = path_generator_.getCoefficient();
300 double get_time_var = 0.0;
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);
314 double shift_offset = - 5.0;
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;
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_);
338 double *get_arg_ = (
double *)arg;
339 initHeart(move_time, start, get_arg_[0], get_arg_[1], get_arg_[2]);
345 return drawHeart(tick);
virtual void setOption(const void *arg)
virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg)
TaskWaypoint drawLine(double time_var)
void initRhombus(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position)
TaskWaypoint drawRhombus(double time_var)
virtual void setOption(const void *arg)
virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg)
virtual void setOption(const void *arg)
TaskWaypoint drawCircle(double time_var)
void initHeart(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position)
virtual TaskWaypoint getTaskWaypoint(double tick)
virtual void setOption(const void *arg)
void initCircle(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position)
virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg)
virtual TaskWaypoint getTaskWaypoint(double tick)
virtual TaskWaypoint getTaskWaypoint(double tick)
Eigen::Vector3d acceleration
virtual TaskWaypoint getTaskWaypoint(double tick)
Eigen::Matrix3d orientation
virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg)
TaskWaypoint drawHeart(double tick)
void initLine(double move_time, TaskWaypoint start, TaskWaypoint delta)