Factor for Discrete Euler Poincare' equation in SE3 for Simple Helicopter dynamics with correct dertivatives
Duy-Nguyen Ta
parent
224be276e4
commit
e4a9c398c8
|
|
@ -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<LieVector, LieVector, Pose3> {
|
||||
|
||||
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<LieVector, LieVector, Pose3> 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>(
|
||||
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<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none,
|
||||
boost::optional<Matrix&> 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 */
|
||||
|
|
|
|||
|
|
@ -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<Vector(const LieVector&)>(
|
||||
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<Vector(const Pose3&, const Pose3&, const LieVector&)>(
|
||||
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<Vector(const Pose3&, const Pose3&, const LieVector&)>(
|
||||
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<Vector(const Pose3&, const Pose3&, const LieVector&)>(
|
||||
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<Vector(const LieVector&, const LieVector&, const Pose3&)>(
|
||||
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<Vector(const LieVector&, const LieVector&, const Pose3&)>(
|
||||
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<Vector(const LieVector&, const LieVector&, const Pose3&)>(
|
||||
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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue