diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index fd572cf24..f36d2dbe6 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -45,24 +45,24 @@ public: static const bool debug = false; - Matrix gkxiHgk, gkxiHexpxi; - Pose3 gkxi = gk.compose(Pose3::Expmap(h_*xik), gkxiHgk, gkxiHexpxi); + Matrix D_gkxi_gk, D_gkxi_exphxi; + Pose3 gkxi = gk.compose(Pose3::Expmap(h_*xik), D_gkxi_gk, D_gkxi_exphxi); - Matrix hxHgk1, hxHgkxi; - Pose3 hx = gkxi.between(gk1, hxHgkxi, hxHgk1); + Matrix D_hx_gk1, D_hx_gkxi; + Pose3 hx = gkxi.between(gk1, D_hx_gkxi, D_hx_gk1); if (H1) { - *H1 = hxHgk1; + *H1 = D_hx_gk1; } if (H2) { - Matrix hxHgk = hxHgkxi*gkxiHgk; - *H2 = hxHgk; + Matrix D_hx_gk = D_hx_gkxi * D_gkxi_gk; + *H2 = D_hx_gk; } if (H3) { - Matrix expxiHxi = Pose3::dExpInv_TLN(xik); - Matrix hxHxi = hxHgkxi*gkxiHexpxi*expxiHxi; - *H3 = hxHxi; + Matrix D_exphxi_xi = Pose3::dExpInv_exp(h_*xik)*h_; + Matrix D_hx_xi = D_hx_gkxi * D_gkxi_exphxi * D_exphxi_xi; + *H3 = D_hx_xi; } return Pose3::Logmap(hx); @@ -70,4 +70,86 @@ public: }; +/** + * Implement the Discrete Euler-Poincare' equation: + */ +class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3 { + + double h_; /// time step + Matrix Inertia_; /// Inertia tensors Inertia = [ J 0; 0 M ] + Vector Fu_; /// F is the 6xc Control matrix, where c is the number of control variables uk, which directly change the vehicle pose (e.g., gas/brake/speed) + /// F(.) is actually a function of the shape variables, which do not change the pose, but affect the vehicle's shape, e.g. steering wheel. + /// Fu_ encodes everything we need to know about the vehicle's dynamics. + double m_; /// mass. For gravity external force f_ext, which has a fixed formula in this case. + + // TODO: Fk_ and f_ext should be generalized as functions (factor nodes) on control signals and poses/velocities. + // This might be needed in control or system identification problems. + // We treat them as constant here, since the control inputs are to specify. + + typedef NoiseModelFactor3 Base; + +public: + DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey, + double h, const Matrix& Inertia, const Vector& Fu, double m, + double mu = 1000.0) : + Base(noiseModel::Constrained::All(Pose3::Dim()*3, fabs(mu)), xiKey1, xiKey_1, gKey), + h_(h), Inertia_(Inertia), Fu_(Fu), m_(m) { + } + virtual ~DiscreteEulerPoincareHelicopter() {} + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new DiscreteEulerPoincareHelicopter(*this))); } + + /** DEP, with optional derivatives + * pk - pk_1 - h_*Fu_ - h_*f_ext = 0 + * where pk = CT_TLN(h*xi_k)*Inertia*xi_k + * pk_1 = CT_TLN(-h*xi_k_1)*Inertia*xi_k_1 + * */ + Vector evaluateError(const LieVector& xik, const LieVector& xik_1, const Pose3& gk, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const { + + static const bool debug = false; + + Vector muk = Inertia_*xik; + Vector muk_1 = Inertia_*xik_1; + + // Apply the inverse right-trivialized tangent (derivative) map of the exponential map, + // using the trapezoidal Lie-Newmark (TLN) scheme, to a vector. + // TLN is just a first order approximation of the dExpInv_exp above, detailed in [Kobilarov09siggraph] + // C_TLN formula: I6 - 1/2 ad[xi]. + Matrix D_adjThxik_muk, D_adjThxik1_muk1; + Vector pk = muk - 0.5*Pose3::adjointTranspose(h_*xik, muk, D_adjThxik_muk); + Vector pk_1 = muk_1 - 0.5*Pose3::adjointTranspose(-h_*xik_1, muk_1, D_adjThxik1_muk1); + + Matrix D_gravityBody_gk; + Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk); + Vector f_ext = Vector_(6, 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z()); + + Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext; + + if (H1) { + Matrix D_pik_xi = Inertia_-0.5*(h_*D_adjThxik_muk + Pose3::adjointMap(h_*xik).transpose()*Inertia_); + *H1 = D_pik_xi; + } + + if (H2) { + Matrix D_pik1_xik1 = Inertia_-0.5*(-h_*D_adjThxik1_muk1 + Pose3::adjointMap(-h_*xik_1).transpose()*Inertia_); + *H2 = -D_pik1_xik1; + } + + if (H3) { + *H3 = zeros(6,6); + insertSub(*H3, -h_*D_gravityBody_gk, 3, 0); + } + + return hx; + } + +}; + + } /* namespace gtsam */ diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index 2047b10cd..4c052f704 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -13,16 +13,41 @@ using namespace gtsam; using namespace gtsam::symbol_shorthand; const double tol=1e-5; -const double h = 1.0; +const double h = 0.01; const double deg2rad = M_PI/180.0; -Pose3 g1(Rot3::ypr(deg2rad*10.0, deg2rad*20.0, deg2rad*30.0), Point3(100.0, 200.0, 300.0)); -LieVector v1(Vector_(6, 0.1, 0.05, 0.02, 10.0, 20.0, 30.0)); -Pose3 g2(g1.retract(h*v1, Pose3::EXPMAP)); +//Pose3 g1(Rot3::ypr(deg2rad*10.0, deg2rad*20.0, deg2rad*30.0), Point3(100.0, 200.0, 300.0)); +Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0)); +//LieVector v1(Vector_(6, 0.1, 0.05, 0.02, 10.0, 20.0, 30.0)); +LieVector V1_w(Vector_(6, 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0)); +LieVector V1_g1 = g1.inverse().Adjoint(V1_w); +Pose3 g2(g1.retract(h*V1_g1, Pose3::EXPMAP)); +//LieVector v2 = Pose3::Logmap(g1.between(g2)); + +double mass = 100.0; +Vector gamma2 = Vector_(2, 0.0, 0.0); // no shape +Vector u2 = Vector_(2, 0.0, 0.0); // no control at time 2 +double distT = 1.0; // distance from the body-centered x axis to the big top motor +double distR = 5.0; // distance from the body-centered z axis to the small motor +Matrix Mass = diag(Vector_(3, mass, mass, mass)); +Matrix Inertia = diag(Vector_(6, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, 2.0/5.0*mass*distR*distR, mass, mass, mass)); + +Vector computeFu(const Vector& gamma, const Vector& control) { + double gamma_r = gamma(0), gamma_p = gamma(1); + + Matrix F = Matrix_(6, 2, distT*sin(gamma_r), 0.0, + distT*sin(gamma_p*cos(gamma_r)), 0.0, + 0.0, distR, + sin(gamma_p)*cos(gamma_r), 0.0, + -sin(gamma_r), -1.0, + cos(gamma_p)*sin(gamma_r), 0.0 + ); + return F*control; +} /* ************************************************************************* */ Vector testExpmapDeriv(const LieVector& v) { - return Pose3::Logmap(Pose3::Expmap(-v1)*Pose3::Expmap(v1+v)); + return Pose3::Logmap(Pose3::Expmap(-h*V1_g1)*Pose3::Expmap(h*V1_g1+v)); } TEST(Reconstruction, ExpmapInvDeriv) { @@ -30,10 +55,10 @@ TEST(Reconstruction, ExpmapInvDeriv) { boost::function( boost::bind(testExpmapDeriv, _1) ), - LieVector(Vector::Zero(6)), 1e-3 + LieVector(Vector::Zero(6)), 1e-5 ); - Matrix dExpInv = Pose3::dExpInv_TLN(v1); - EXPECT(assert_equal(numericalExpmap, dExpInv, 5e-1)); + Matrix dExpInv = Pose3::dExpInv_exp(h*V1_g1); + EXPECT(assert_equal(numericalExpmap, dExpInv, 1e-2)); } /* ************************************************************************* */ @@ -43,33 +68,87 @@ TEST( Reconstruction, evaluateError) { // verify error function Matrix H1, H2, H3; - EXPECT(assert_equal(zero(6), constraint.evaluateError(g2, g1, v1, H1, H2, H3), tol)); + EXPECT(assert_equal(zero(6), constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol)); Matrix numericalH1 = numericalDerivative31( boost::function( boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) ), - g2, g1, v1, 1e-5 + g2, g1, V1_g1, 1e-5 ); Matrix numericalH2 = numericalDerivative32( boost::function( boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) ), - g2, g1, v1, 1e-5 + g2, g1, V1_g1, 1e-5 ); Matrix numericalH3 = numericalDerivative33( boost::function( boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) ), - g2, g1, v1, 1e-5 + g2, g1, V1_g1, 1e-5 ); EXPECT(assert_equal(numericalH1,H1,1e-5)); EXPECT(assert_equal(numericalH2,H2,1e-5)); - EXPECT(assert_equal(numericalH3,H3,5e-1)); + EXPECT(assert_equal(numericalH3,H3,1e-5)); +} + +/* ************************************************************************* */ +// Implement Newton-Euler equation for rigid body dynamics +Vector newtonEuler(const Vector& Vb, const Vector& Fb, const Matrix& Inertia) { + Matrix W = Pose3::adjointMap(Vector_(6, Vb(0), Vb(1), Vb(2), 0., 0., 0.)); + Vector dV = Inertia.inverse()*(Fb - W*Inertia*Vb); + return dV; +} + +TEST( DiscreteEulerPoincareHelicopter, evaluateError) { + Vector Fu = computeFu(gamma2, u2); + Vector fGravity_g1 = zero(6); subInsert(fGravity_g1, g1.rotation().unrotate(Point3(0.0, 0.0, -mass*9.81)).vector(), 3); // gravity force in g1 frame + Vector Fb = Fu+fGravity_g1; + + Vector dV = newtonEuler(V1_g1, Fb, Inertia); + Vector V2_g1 = dV*h + V1_g1; + Pose3 g21 = g2.between(g1); + Vector V2_g2 = g21.Adjoint(V2_g1); // convert the new velocity to g2's frame + + LieVector expectedv2(V2_g2); + + // hard constraints don't need a noise model + DiscreteEulerPoincareHelicopter constraint(V(2), V(1), G(2), h, + Inertia, Fu, mass); + + // verify error function + Matrix H1, H2, H3; + EXPECT(assert_equal(zero(6), constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0)); + + Matrix numericalH1 = numericalDerivative31( + boost::function( + boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) + ), + expectedv2, V1_g1, g2, 1e-5 + ); + + Matrix numericalH2 = numericalDerivative32( + boost::function( + boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) + ), + expectedv2, V1_g1, g2, 1e-5 + ); + + Matrix numericalH3 = numericalDerivative33( + boost::function( + boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) + ), + expectedv2, V1_g1, g2, 1e-5 + ); + + EXPECT(assert_equal(numericalH1,H1,1e-5)); + EXPECT(assert_equal(numericalH2,H2,1e-5)); + EXPECT(assert_equal(numericalH3,H3,5e-5)); } /* ************************************************************************* */