-
Notifications
You must be signed in to change notification settings - Fork 178
/
circle_trajectory_helper.cpp
141 lines (119 loc) · 5.69 KB
/
circle_trajectory_helper.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
#include "trajectory_generation_helper/circle_trajectory_helper.h"
#include <quadrotor_common/math_common.h>
#include <quadrotor_common/trajectory_point.h>
namespace trajectory_generation_helper {
namespace circles {
quadrotor_common::Trajectory computeHorizontalCircleTrajectory(
const Eigen::Vector3d center, const double radius, const double speed,
const double phi_start, const double phi_end,
const double sampling_frequency) {
/*
* We use a coordinate system with x to the front, y to the left, and z up.
* When setting phi_start = 0.0 the start point will be at
* (radius, 0, 0) + center
* If phi_end > phi_start the trajectory is going counter clock wise
* otherwise it is going clockwise
*/
quadrotor_common::Trajectory trajectory;
trajectory.trajectory_type =
quadrotor_common::Trajectory::TrajectoryType::GENERAL;
const double phi_total = phi_end - phi_start;
const double direction = phi_total / fabs(phi_total);
const double omega = direction * fabs(speed / radius);
const double angle_step = fabs(omega / sampling_frequency);
for (double d_phi = 0.0; d_phi < fabs(phi_total); d_phi += angle_step) {
const double phi = phi_start + direction * d_phi;
const double cos_phi = cos(phi);
const double sin_phi = sin(phi);
quadrotor_common::TrajectoryPoint point;
point.time_from_start = ros::Duration(fabs(d_phi / omega));
point.position = radius * Eigen::Vector3d(cos_phi, sin_phi, 0.0) + center;
point.velocity = radius * omega * Eigen::Vector3d(-sin_phi, cos_phi, 0.0);
point.acceleration =
radius * pow(omega, 2.0) * Eigen::Vector3d(-cos_phi, -sin_phi, 0.0);
point.jerk =
radius * pow(omega, 3.0) * Eigen::Vector3d(sin_phi, -cos_phi, 0.0);
point.snap =
radius * pow(omega, 4.0) * Eigen::Vector3d(cos_phi, sin_phi, 0.0);
trajectory.points.push_back(point);
}
// Add last point at phi_end
const double phi = phi_start + phi_total;
const double cos_phi = cos(phi);
const double sin_phi = sin(phi);
quadrotor_common::TrajectoryPoint point;
point.time_from_start = ros::Duration(fabs(phi_total / omega));
point.position = radius * Eigen::Vector3d(cos_phi, sin_phi, 0.0) + center;
point.velocity = radius * omega * Eigen::Vector3d(-sin_phi, cos_phi, 0.0);
point.acceleration =
radius * pow(omega, 2.0) * Eigen::Vector3d(-cos_phi, -sin_phi, 0.0);
point.jerk =
radius * pow(omega, 3.0) * Eigen::Vector3d(sin_phi, -cos_phi, 0.0);
point.snap =
radius * pow(omega, 4.0) * Eigen::Vector3d(cos_phi, sin_phi, 0.0);
trajectory.points.push_back(point);
return trajectory;
}
quadrotor_common::Trajectory computeVerticalCircleTrajectory(
const Eigen::Vector3d center, const double orientation, const double radius,
const double speed, const double phi_start, const double phi_end,
const double sampling_frequency) {
/*
* We use a coordinate system with x to the front, y to the left, and z up.
* Orientation is the angle by which the circle is rotated about the z-axis
* If the orientation = 0.0 then the circle is in the x-z plane
* When setting phi_start = 0.0 and orientation = 0.0, the start point will
* be at (radius, 0, 0) + center
* If phi_end > phi_start the circle is going in positive direction around
* the y-axis rotated by orientation. Otherwise it is going in negative
* direction
*/
quadrotor_common::Trajectory trajectory;
trajectory.trajectory_type =
quadrotor_common::Trajectory::TrajectoryType::GENERAL;
const double phi_total = phi_end - phi_start;
const double direction = phi_total / fabs(phi_total);
const double omega = direction * fabs(speed / radius);
const double angle_step = fabs(omega / sampling_frequency);
const Eigen::Quaterniond q_ori = Eigen::Quaterniond(
Eigen::AngleAxisd(quadrotor_common::wrapMinusPiToPi(orientation),
Eigen::Vector3d::UnitZ()));
for (double d_phi = 0.0; d_phi < fabs(phi_total); d_phi += angle_step) {
const double phi = phi_start + direction * d_phi;
const double cos_phi = cos(phi);
const double sin_phi = sin(phi);
quadrotor_common::TrajectoryPoint point;
point.time_from_start = ros::Duration(fabs(d_phi / omega));
point.position =
q_ori * (radius * Eigen::Vector3d(cos_phi, 0.0, -sin_phi)) + center;
point.velocity =
q_ori * (radius * omega * Eigen::Vector3d(-sin_phi, 0.0, -cos_phi));
point.acceleration = q_ori * (radius * pow(omega, 2.0) *
Eigen::Vector3d(-cos_phi, 0.0, sin_phi));
point.jerk = q_ori * (radius * pow(omega, 3.0) *
Eigen::Vector3d(sin_phi, 0.0, cos_phi));
point.snap = q_ori * (radius * pow(omega, 4.0) *
Eigen::Vector3d(cos_phi, 0.0, -sin_phi));
trajectory.points.push_back(point);
}
// Add last point at phi_end
const double phi = phi_start + phi_total;
const double cos_phi = cos(phi);
const double sin_phi = sin(phi);
quadrotor_common::TrajectoryPoint point;
point.time_from_start = ros::Duration(fabs(phi_total / omega));
point.position =
q_ori * (radius * Eigen::Vector3d(cos_phi, 0.0, -sin_phi)) + center;
point.velocity =
q_ori * (radius * omega * Eigen::Vector3d(-sin_phi, 0.0, -cos_phi));
point.acceleration = q_ori * (radius * pow(omega, 2.0) *
Eigen::Vector3d(-cos_phi, 0.0, sin_phi));
point.jerk = q_ori * (radius * pow(omega, 3.0) *
Eigen::Vector3d(sin_phi, 0.0, cos_phi));
point.snap = q_ori * (radius * pow(omega, 4.0) *
Eigen::Vector3d(cos_phi, 0.0, -sin_phi));
trajectory.points.push_back(point);
return trajectory;
}
} // namespace circles
} // namespace trajectory_generation_helper