PoseRTV.cpp
Go to the documentation of this file.
1 
7 #include <gtsam/geometry/Pose2.h>
8 #include <gtsam/base/Vector.h>
9 
10 namespace gtsam {
11 
12 using namespace std;
13 
14 static const Vector kGravity = Vector::Unit(3,2)*9.81;
15 
16 /* ************************************************************************* */
17 double bound(double a, double min, double max) {
18  if (a < min) return min;
19  else if (a > max) return max;
20  else return a;
21 }
22 
23 /* ************************************************************************* */
24 PoseRTV::PoseRTV(double roll, double pitch, double yaw, double x, double y,
25  double z, double vx, double vy, double vz) :
26  Base(Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)),
27  Velocity3(vx, vy, vz)) {
28 }
29 
30 /* ************************************************************************* */
31 PoseRTV::PoseRTV(const Vector& rtv) :
32  Base(Pose3(Rot3::RzRyRx(rtv.head(3)), Point3(rtv.segment(3, 3))),
33  Velocity3(rtv.tail(3))) {
34 }
35 
36 /* ************************************************************************* */
38  Vector rtv(9);
39  rtv.head(3) = rotation().xyz();
40  rtv.segment(3,3) = translation();
41  rtv.tail(3) = velocity();
42  return rtv;
43 }
44 
45 /* ************************************************************************* */
46 bool PoseRTV::equals(const PoseRTV& other, double tol) const {
47  return pose().equals(other.pose(), tol)
48  && equal_with_abs_tol(velocity(), other.velocity(), tol);
49 }
50 
51 /* ************************************************************************* */
52 void PoseRTV::print(const string& s) const {
53  cout << s << ":" << endl;
54  gtsam::print((Vector)R().xyz(), " R:rpy");
55  cout << " T" << t().transpose() << endl;
56  gtsam::print((Vector)velocity(), " V");
57 }
58 
59 /* ************************************************************************* */
60 PoseRTV PoseRTV::planarDynamics(double vel_rate, double heading_rate,
61  double max_accel, double dt) const {
62 
63  // split out initial state
64  const Rot3& r1 = R();
65  const Velocity3& v1 = v();
66 
67  // Update vehicle heading
68  Rot3 r2 = r1.retract((Vector(3) << 0.0, 0.0, heading_rate * dt).finished());
69  const double yaw2 = r2.ypr()(0);
70 
71  // Update vehicle position
72  const double mag_v1 = v1.norm();
73 
74  // FIXME: this doesn't account for direction in velocity bounds
75  double dv = bound(vel_rate - mag_v1, - (max_accel * dt), max_accel * dt);
76  double mag_v2 = mag_v1 + dv;
77  Velocity3 v2 = mag_v2 * Velocity3(cos(yaw2), sin(yaw2), 0.0);
78 
79  Point3 t2 = translationIntegration(r2, v2, dt);
80 
81  return PoseRTV(r2, t2, v2);
82 }
83 
84 /* ************************************************************************* */
86  double pitch_rate, double heading_rate, double lift_control, double dt) const {
87  // split out initial state
88  const Rot3& r1 = R();
89  const Velocity3& v1 = v();
90 
91  // Update vehicle heading (and normalise yaw)
92  Vector rot_rates = (Vector(3) << 0.0, pitch_rate, heading_rate).finished();
93  Rot3 r2 = r1.retract(rot_rates*dt);
94 
95  // Work out dynamics on platform
96  const double thrust = 50.0;
97  const double lift = 50.0;
98  const double drag = 0.1;
99  double yaw2 = r2.yaw();
100  double pitch2 = r2.pitch();
101  double forward_accel = -thrust * sin(pitch2); // r2, pitch (in global frame?) controls forward force
102  double loss_lift = lift*std::abs(sin(pitch2));
103  Rot3 yaw_correction_bn = Rot3::Yaw(yaw2);
104  Point3 forward(forward_accel, 0.0, 0.0);
105  Vector Acc_n =
106  yaw_correction_bn.rotate(forward) // applies locally forward force in the global frame
107  - drag * (Vector(3) << v1.x(), v1.y(), 0.0).finished() // drag term dependent on v1
108  + Vector::Unit(3,2)*(loss_lift - lift_control); // falling due to lift lost from pitch
109 
110  // Update Vehicle Position and Velocity
111  Velocity3 v2 = v1 + Velocity3(Acc_n * dt);
112  Point3 t2 = translationIntegration(r2, v2, dt);
113 
114  return PoseRTV(r2, t2, v2);
115 }
116 
117 /* ************************************************************************* */
119  const Vector& accel, const Vector& gyro, double dt) const {
120  // Integrate Attitude Equations
121  Rot3 r2 = rotation().retract(gyro * dt);
122 
123  // Integrate Velocity Equations
124  Velocity3 v2 = velocity() + Velocity3(dt * (r2.matrix() * accel + kGravity));
125 
126  // Integrate Position Equations
127  Point3 t2 = translationIntegration(r2, v2, dt);
128 
129  return PoseRTV(t2, r2, v2);
130 }
131 
132 /* ************************************************************************* */
133 Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
134  // split out states
135  const Rot3 &r1 = R(), &r2 = x2.R();
136  const Velocity3 &v1 = v(), &v2 = x2.v();
137 
138  Vector6 imu;
139 
140  // acceleration
141  Vector3 accel = (v2-v1) / dt;
142  imu.head<3>() = r2.transpose() * (accel - kGravity);
143 
144  // rotation rates
145  // just using euler angles based on matlab code
146  // FIXME: this is silly - we shouldn't use differences in Euler angles
147  Matrix Enb = RRTMnb(r1);
148  Vector3 euler1 = r1.xyz(), euler2 = r2.xyz();
149  Vector3 dR = euler2 - euler1;
150 
151  // normalize yaw in difference (as per Mitch's code)
152  dR(2) = Rot2::fromAngle(dR(2)).theta();
153  dR /= dt;
154  imu.tail<3>() = Enb * dR;
155 // imu.tail(3) = r1.transpose() * dR;
156 
157  return imu;
158 }
159 
160 /* ************************************************************************* */
161 Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, double dt) const {
162  // predict point for constraint
163  // NOTE: uses simple Euler approach for prediction
164  Point3 pred_t2 = t() + Point3(v2 * dt);
165  return pred_t2;
166 }
167 
168 /* ************************************************************************* */
171  Matrix36 D_t1_pose, D_t2_other;
172  const Point3 t1 = pose().translation(H1 ? &D_t1_pose : 0);
173  const Point3 t2 = other.pose().translation(H2 ? &D_t2_other : 0);
174  Matrix13 D_d_t1, D_d_t2;
175  double d = distance3(t1, t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0);
176  if (H1) *H1 << D_d_t1 * D_t1_pose, 0,0,0;
177  if (H2) *H2 << D_d_t2 * D_t2_other, 0,0,0;
178  return d;
179 }
180 
181 /* ************************************************************************* */
183  OptionalJacobian<9, 6> Dtrans) const {
184 
185  // Pose3 transform is just compose
186  Matrix6 D_newpose_trans, D_newpose_pose;
187  Pose3 newpose = trans.compose(pose(), D_newpose_trans, D_newpose_pose);
188 
189  // Note that we rotate the velocity
190  Matrix3 D_newvel_R, D_newvel_v;
191  Velocity3 newvel = trans.rotation().rotate(Point3(velocity()), D_newvel_R, D_newvel_v);
192 
193  if (Dglobal) {
194  Dglobal->setZero();
195  Dglobal->topLeftCorner<6,6>() = D_newpose_pose;
196  Dglobal->bottomRightCorner<3,3>() = D_newvel_v;
197  }
198 
199  if (Dtrans) {
200  Dtrans->setZero();
201  Dtrans->topLeftCorner<6,6>() = D_newpose_trans;
202  Dtrans->bottomLeftCorner<3,3>() = D_newvel_R;
203  }
204  return PoseRTV(newpose, newvel);
205 }
206 
207 /* ************************************************************************* */
209  assert(euler.size() == 3);
210  const double s1 = sin(euler.x()), c1 = cos(euler.x());
211  const double t2 = tan(euler.y()), c2 = cos(euler.y());
212  Matrix Ebn(3,3);
213  Ebn << 1.0, s1 * t2, c1 * t2,
214  0.0, c1, -s1,
215  0.0, s1 / c2, c1 / c2;
216  return Ebn;
217 }
218 
219 /* ************************************************************************* */
221  return PoseRTV::RRTMbn(att.rpy());
222 }
223 
224 /* ************************************************************************* */
226  Matrix Enb(3,3);
227  const double s1 = sin(euler.x()), c1 = cos(euler.x());
228  const double s2 = sin(euler.y()), c2 = cos(euler.y());
229  Enb << 1.0, 0.0, -s2,
230  0.0, c1, s1*c2,
231  0.0, -s1, c1*c2;
232  return Enb;
233 }
234 
235 /* ************************************************************************* */
237  return PoseRTV::RRTMnb(att.rpy());
238 }
239 
240 /* ************************************************************************* */
241 } // \namespace gtsam
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
void print(const std::string &s="") const
Definition: PoseRTV.cpp:52
#define max(a, b)
Definition: datatypes.h:20
static Rot3 RzRyRx(double x, double y, double z, OptionalJacobian< 3, 1 > Hx={}, OptionalJacobian< 3, 1 > Hy={}, OptionalJacobian< 3, 1 > Hz={})
Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.
Definition: Rot3M.cpp:84
StridedVectorType vy(make_vector(y, *n, std::abs(*incy)))
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
const Point3 & translation() const
Definition: PoseRTV.h:62
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:308
Scalar * y
Vector v2
Eigen::Vector3d Vector3
Definition: Vector.h:43
Vector v1
const Velocity3 & velocity() const
Definition: PoseRTV.h:64
PoseRTV planarDynamics(double vel_rate, double heading_rate, double max_accel, double dt) const
Definition: PoseRTV.cpp:60
#define min(a, b)
Definition: datatypes.h:19
Vector3 Velocity3
Velocity is currently typedef&#39;d to Vector3.
Definition: NavState.h:28
double pitch(OptionalJacobian< 1, 3 > H={}) const
Definition: Rot3.cpp:206
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
const Rot3 & R() const
Definition: PoseRTV.h:59
Definition: BFloat16.h:88
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
const Pose3 & pose() const
Definition: PoseRTV.h:56
double bound(double a, double min, double max)
Definition: PoseRTV.cpp:17
bool equals(const Pose3 &pose, double tol=1e-9) const
assert equality up to a tolerance
Definition: Pose3.cpp:157
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
Matrix trans(const Matrix &A)
Definition: base/Matrix.h:241
static Matrix RRTMnb(const Vector3 &euler)
Definition: PoseRTV.cpp:225
PoseRTV flyingDynamics(double pitch_rate, double heading_rate, double lift_control, double dt) const
Definition: PoseRTV.cpp:85
const double dt
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
Definition: Rot3.h:174
Eigen::VectorXd Vector
Definition: Vector.h:38
Vector3 xyz(OptionalJacobian< 3, 3 > H={}) const
Definition: Rot3.cpp:160
double range(const PoseRTV &other, OptionalJacobian< 1, 9 > H1={}, OptionalJacobian< 1, 9 > H2={}) const
Definition: PoseRTV.cpp:169
bool equals(const PoseRTV &other, double tol=1e-6) const
Definition: PoseRTV.cpp:46
PoseRTV transformed_from(const Pose3 &trans, ChartJacobian Dglobal={}, OptionalJacobian< 9, 6 > Dtrans={}) const
Definition: PoseRTV.cpp:182
StridedVectorType vx(make_vector(x, *n, std::abs(*incx)))
Vector3 rpy(OptionalJacobian< 3, 3 > H={}) const
Definition: Rot3.cpp:191
RealScalar s
static const double r2
static Matrix RRTMbn(const Vector3 &euler)
Definition: PoseRTV.cpp:208
Class compose(const Class &g) const
Definition: Lie.h:48
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:80
Pose3 with translational velocity.
traits
Definition: chartTesting.h:28
typedef and functions to augment Eigen&#39;s VectorXd
ProductLieGroup< Pose3, Velocity3 > Base
Definition: PoseRTV.h:26
const Rot3 & rotation() const
Definition: PoseRTV.h:63
static const double r1
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition: Lie.h:131
Vector vector() const
Definition: PoseRTV.cpp:37
Matrix3 matrix() const
Definition: Rot3M.cpp:218
EIGEN_DEVICE_FUNC const TanReturnType tan() const
PoseRTV generalDynamics(const Vector &accel, const Vector &gyro, double dt) const
General Dynamics update - supply control inputs in body frame.
Definition: PoseRTV.cpp:118
static const Vector kGravity
Definition: PoseRTV.cpp:14
Point3 translationIntegration(const Rot3 &r2, const Velocity3 &v2, double dt) const
Definition: PoseRTV.cpp:161
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
Definition: Rot2.h:61
const Point3 & t() const
Definition: PoseRTV.h:58
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
double yaw(OptionalJacobian< 1, 3 > H={}) const
Definition: Rot3.cpp:218
Vector3 ypr(OptionalJacobian< 3, 3 > H={}) const
Definition: Rot3.cpp:183
const G double tol
Definition: Group.h:86
Vector3 Point3
Definition: Point3.h:38
double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > H1, OptionalJacobian< 1, 3 > H2)
distance between two points
Definition: Point3.cpp:27
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Definition: Rot3M.cpp:148
2D Pose
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
#define abs(x)
Definition: datatypes.h:17
Vector6 imuPrediction(const PoseRTV &x2, double dt) const
Definition: PoseRTV.cpp:133
const Velocity3 & v() const
Definition: PoseRTV.h:57
double theta() const
Definition: Rot2.h:186


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:15