1/**
2 * @file PoseRTV.cpp
3 * @author Alex Cunningham
4 */
5
6#include <gtsam_unstable/dynamics/PoseRTV.h>
7#include <gtsam/geometry/Pose2.h>
8#include <gtsam/base/Vector.h>
9
10#include <cassert>
11
12namespace gtsam {
13
14using namespace std;
15
16static const Vector kGravity = Vector::Unit(newSize: 3,i: 2)*9.81;
17
18/* ************************************************************************* */
19double bound(double a, double min, double max) {
20 if (a < min) return min;
21 else if (a > max) return max;
22 else return a;
23}
24
25/* ************************************************************************* */
26PoseRTV::PoseRTV(double roll, double pitch, double yaw, double x, double y,
27 double z, double vx, double vy, double vz) :
28 Base(Pose3(Rot3::RzRyRx(x: roll, y: pitch, z: yaw), Point3(x, y, z)),
29 Velocity3(vx, vy, vz)) {
30}
31
32/* ************************************************************************* */
33PoseRTV::PoseRTV(const Vector& rtv) :
34 Base(Pose3(Rot3::RzRyRx(xyz: rtv.head(n: 3)), Point3(rtv.segment(start: 3, n: 3))),
35 Velocity3(rtv.tail(n: 3))) {
36}
37
38/* ************************************************************************* */
39Vector PoseRTV::vector() const {
40 Vector rtv(9);
41 rtv.head(n: 3) = rotation().xyz();
42 rtv.segment(start: 3,n: 3) = translation();
43 rtv.tail(n: 3) = velocity();
44 return rtv;
45}
46
47/* ************************************************************************* */
48bool PoseRTV::equals(const PoseRTV& other, double tol) const {
49 return pose().equals(pose: other.pose(), tol)
50 && equal_with_abs_tol(A: velocity(), B: other.velocity(), tol);
51}
52
53/* ************************************************************************* */
54void PoseRTV::print(const string& s) const {
55 cout << s << ":" << endl;
56 gtsam::print(v: (Vector)R().xyz(), s: " R:rpy");
57 cout << " T" << t().transpose() << endl;
58 gtsam::print(v: (Vector)velocity(), s: " V");
59}
60
61/* ************************************************************************* */
62PoseRTV PoseRTV::planarDynamics(double vel_rate, double heading_rate,
63 double max_accel, double dt) const {
64
65 // split out initial state
66 const Rot3& r1 = R();
67 const Velocity3& v1 = v();
68
69 // Update vehicle heading
70 Rot3 r2 = r1.retract(v: (Vector(3) << 0.0, 0.0, heading_rate * dt).finished());
71 const double yaw2 = r2.ypr()(0);
72
73 // Update vehicle position
74 const double mag_v1 = v1.norm();
75
76 // FIXME: this doesn't account for direction in velocity bounds
77 double dv = bound(a: vel_rate - mag_v1, min: - (max_accel * dt), max: max_accel * dt);
78 double mag_v2 = mag_v1 + dv;
79 Velocity3 v2 = mag_v2 * Velocity3(cos(x: yaw2), sin(x: yaw2), 0.0);
80
81 Point3 t2 = translationIntegration(r2, v2, dt);
82
83 return PoseRTV(r2, t2, v2);
84}
85
86/* ************************************************************************* */
87PoseRTV PoseRTV::flyingDynamics(
88 double pitch_rate, double heading_rate, double lift_control, double dt) const {
89 // split out initial state
90 const Rot3& r1 = R();
91 const Velocity3& v1 = v();
92
93 // Update vehicle heading (and normalise yaw)
94 Vector rot_rates = (Vector(3) << 0.0, pitch_rate, heading_rate).finished();
95 Rot3 r2 = r1.retract(v: rot_rates*dt);
96
97 // Work out dynamics on platform
98 const double thrust = 50.0;
99 const double lift = 50.0;
100 const double drag = 0.1;
101 double yaw2 = r2.yaw();
102 double pitch2 = r2.pitch();
103 double forward_accel = -thrust * sin(x: pitch2); // r2, pitch (in global frame?) controls forward force
104 double loss_lift = lift*std::abs(x: sin(x: pitch2));
105 Rot3 yaw_correction_bn = Rot3::Yaw(t: yaw2);
106 Point3 forward(forward_accel, 0.0, 0.0);
107 Vector Acc_n =
108 yaw_correction_bn.rotate(p: forward) // applies locally forward force in the global frame
109 - drag * (Vector(3) << v1.x(), v1.y(), 0.0).finished() // drag term dependent on v1
110 + Vector::Unit(newSize: 3,i: 2)*(loss_lift - lift_control); // falling due to lift lost from pitch
111
112 // Update Vehicle Position and Velocity
113 Velocity3 v2 = v1 + Velocity3(Acc_n * dt);
114 Point3 t2 = translationIntegration(r2, v2, dt);
115
116 return PoseRTV(r2, t2, v2);
117}
118
119/* ************************************************************************* */
120PoseRTV PoseRTV::generalDynamics(
121 const Vector& accel, const Vector& gyro, double dt) const {
122 // Integrate Attitude Equations
123 Rot3 r2 = rotation().retract(v: gyro * dt);
124
125 // Integrate Velocity Equations
126 Velocity3 v2 = velocity() + Velocity3(dt * (r2.matrix() * accel + kGravity));
127
128 // Integrate Position Equations
129 Point3 t2 = translationIntegration(r2, v2, dt);
130
131 return PoseRTV(t2, r2, v2);
132}
133
134/* ************************************************************************* */
135Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
136 // split out states
137 const Rot3 &r1 = R(), &r2 = x2.R();
138 const Velocity3 &v1 = v(), &v2 = x2.v();
139
140 Vector6 imu;
141
142 // acceleration
143 Vector3 accel = (v2-v1) / dt;
144 imu.head<3>() = r2.transpose() * (accel - kGravity);
145
146 // rotation rates
147 // just using euler angles based on matlab code
148 // FIXME: this is silly - we shouldn't use differences in Euler angles
149 Matrix Enb = RRTMnb(att: r1);
150 Vector3 euler1 = r1.xyz(), euler2 = r2.xyz();
151 Vector3 dR = euler2 - euler1;
152
153 // normalize yaw in difference (as per Mitch's code)
154 dR(2) = Rot2::fromAngle(theta: dR(2)).theta();
155 dR /= dt;
156 imu.tail<3>() = Enb * dR;
157// imu.tail(3) = r1.transpose() * dR;
158
159 return imu;
160}
161
162/* ************************************************************************* */
163Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, double dt) const {
164 // predict point for constraint
165 // NOTE: uses simple Euler approach for prediction
166 Point3 pred_t2 = t() + Point3(v2 * dt);
167 return pred_t2;
168}
169
170/* ************************************************************************* */
171double PoseRTV::range(const PoseRTV& other,
172 OptionalJacobian<1,9> H1, OptionalJacobian<1,9> H2) const {
173 Matrix36 D_t1_pose, D_t2_other;
174 const Point3 t1 = pose().translation(Hself: H1 ? &D_t1_pose : 0);
175 const Point3 t2 = other.pose().translation(Hself: H2 ? &D_t2_other : 0);
176 Matrix13 D_d_t1, D_d_t2;
177 double d = distance3(p1: t1, q: t2, H1: H1 ? &D_d_t1 : 0, H2: H2 ? &D_d_t2 : 0);
178 if (H1) *H1 << D_d_t1 * D_t1_pose, 0,0,0;
179 if (H2) *H2 << D_d_t2 * D_t2_other, 0,0,0;
180 return d;
181}
182
183/* ************************************************************************* */
184PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal,
185 OptionalJacobian<9, 6> Dtrans) const {
186
187 // Pose3 transform is just compose
188 Matrix6 D_newpose_trans, D_newpose_pose;
189 Pose3 newpose = trans.compose(g: pose(), H1: D_newpose_trans, H2: D_newpose_pose);
190
191 // Note that we rotate the velocity
192 Matrix3 D_newvel_R, D_newvel_v;
193 Velocity3 newvel = trans.rotation().rotate(p: Point3(velocity()), H1: D_newvel_R, H2: D_newvel_v);
194
195 if (Dglobal) {
196 Dglobal->setZero();
197 Dglobal->topLeftCorner<6,6>() = D_newpose_pose;
198 Dglobal->bottomRightCorner<3,3>() = D_newvel_v;
199 }
200
201 if (Dtrans) {
202 Dtrans->setZero();
203 Dtrans->topLeftCorner<6,6>() = D_newpose_trans;
204 Dtrans->bottomLeftCorner<3,3>() = D_newvel_R;
205 }
206 return PoseRTV(newpose, newvel);
207}
208
209/* ************************************************************************* */
210Matrix PoseRTV::RRTMbn(const Vector3& euler) {
211 assert(euler.size() == 3);
212 const double s1 = sin(x: euler.x()), c1 = cos(x: euler.x());
213 const double t2 = tan(x: euler.y()), c2 = cos(x: euler.y());
214 Matrix Ebn(3,3);
215 Ebn << 1.0, s1 * t2, c1 * t2,
216 0.0, c1, -s1,
217 0.0, s1 / c2, c1 / c2;
218 return Ebn;
219}
220
221/* ************************************************************************* */
222Matrix PoseRTV::RRTMbn(const Rot3& att) {
223 return PoseRTV::RRTMbn(euler: att.rpy());
224}
225
226/* ************************************************************************* */
227Matrix PoseRTV::RRTMnb(const Vector3& euler) {
228 Matrix Enb(3,3);
229 const double s1 = sin(x: euler.x()), c1 = cos(x: euler.x());
230 const double s2 = sin(x: euler.y()), c2 = cos(x: euler.y());
231 Enb << 1.0, 0.0, -s2,
232 0.0, c1, s1*c2,
233 0.0, -s1, c1*c2;
234 return Enb;
235}
236
237/* ************************************************************************* */
238Matrix PoseRTV::RRTMnb(const Rot3& att) {
239 return PoseRTV::RRTMnb(euler: att.rpy());
240}
241
242/* ************************************************************************* */
243} // \namespace gtsam
244