factors for the pendulum discrete mechanics in position-momentum form to use with variational integrator
Duy-Nguyen Ta
parent
6b87b9d307
commit
494885bf1d
|
|
@ -14,11 +14,12 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
//*************************************************************************
|
||||
/**
|
||||
* This class implements the first constraint.
|
||||
* - For explicit Euler method: q_{k+1} = q_k + dt*v_k
|
||||
* - For implicit Euler method: q_{k+1} = q_k + dt*v_{k+1}
|
||||
* - For sympletic Euler method: q_{k+1} = q_k + dt*v_{k+1}
|
||||
* - For explicit Euler method: q_{k+1} = q_k + h*v_k
|
||||
* - For implicit Euler method: q_{k+1} = q_k + h*v_{k+1}
|
||||
* - For sympletic Euler method: q_{k+1} = q_k + h*v_{k+1}
|
||||
*/
|
||||
class PendulumFactor1: public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> {
|
||||
public:
|
||||
|
|
@ -29,22 +30,22 @@ protected:
|
|||
/** default constructor to allow for serialization */
|
||||
PendulumFactor1() {}
|
||||
|
||||
double dt_;
|
||||
double h_; // time step
|
||||
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<PendulumFactor1> shared_ptr;
|
||||
|
||||
///Constructor. k1: q_{k+1}, k: q_k, velKey: velocity variable depending on the chosen method, dt: time step
|
||||
PendulumFactor1(Key k1, Key k, Key velKey, double dt, double mu = 1000.0)
|
||||
: Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), k1, k, velKey), dt_(dt) {}
|
||||
///Constructor. k1: q_{k+1}, k: q_k, velKey: velocity variable depending on the chosen method, h: time step
|
||||
PendulumFactor1(Key k1, Key k, Key velKey, double h, double mu = 1000.0)
|
||||
: Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), k1, k, velKey), h_(h) {}
|
||||
|
||||
/// @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 PendulumFactor1(*this))); }
|
||||
|
||||
/** q_k + dt*v - q_k1 = 0, with optional derivatives */
|
||||
/** q_k + h*v - q_k1 = 0, with optional derivatives */
|
||||
Vector evaluateError(const LieScalar& qk1, const LieScalar& qk, const LieScalar& v,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none,
|
||||
|
|
@ -52,18 +53,19 @@ public:
|
|||
const size_t p = LieScalar::Dim();
|
||||
if (H1) *H1 = -eye(p);
|
||||
if (H2) *H2 = eye(p);
|
||||
if (H3) *H3 = eye(p)*dt_;
|
||||
return qk1.localCoordinates(qk.compose(LieScalar(v*dt_)));
|
||||
if (H3) *H3 = eye(p)*h_;
|
||||
return qk1.localCoordinates(qk.compose(LieScalar(v*h_)));
|
||||
}
|
||||
|
||||
}; // \PendulumFactor1
|
||||
|
||||
|
||||
//*************************************************************************
|
||||
/**
|
||||
* This class implements the second constraint the
|
||||
* - For explicit Euler method: v_{k+1} = v_k - dt*g/L*sin(q_k)
|
||||
* - For implicit Euler method: v_{k+1} = v_k - dt*g/L*sin(q_{k+1})
|
||||
* - For sympletic Euler method: v_{k+1} = v_k - dt*g/L*sin(q_k)
|
||||
* - For explicit Euler method: v_{k+1} = v_k - h*g/L*sin(q_k)
|
||||
* - For implicit Euler method: v_{k+1} = v_k - h*g/L*sin(q_{k+1})
|
||||
* - For sympletic Euler method: v_{k+1} = v_k - h*g/L*sin(q_k)
|
||||
*/
|
||||
class PendulumFactor2: public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> {
|
||||
public:
|
||||
|
|
@ -74,24 +76,24 @@ protected:
|
|||
/** default constructor to allow for serialization */
|
||||
PendulumFactor2() {}
|
||||
|
||||
double dt_;
|
||||
double h_;
|
||||
double g_;
|
||||
double L_;
|
||||
double r_;
|
||||
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<PendulumFactor2 > shared_ptr;
|
||||
|
||||
///Constructor. vk1: v_{k+1}, vk: v_k, qkey: q's key depending on the chosen method, dt: time step
|
||||
PendulumFactor2(Key vk1, Key vk, Key qkey, double dt, double L = 1.0, double g = 9.81, double mu = 1000.0)
|
||||
: Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), vk1, vk, qkey), dt_(dt), g_(g), L_(L) {}
|
||||
///Constructor. vk1: v_{k+1}, vk: v_k, qkey: q's key depending on the chosen method, h: time step
|
||||
PendulumFactor2(Key vk1, Key vk, Key qkey, double h, double r = 1.0, double g = 9.81, double mu = 1000.0)
|
||||
: Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {}
|
||||
|
||||
/// @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 PendulumFactor2(*this))); }
|
||||
|
||||
/** v_k - dt*g/L*sin(q) - v_k1 = 0, with optional derivatives */
|
||||
/** v_k - h*g/L*sin(q) - v_k1 = 0, with optional derivatives */
|
||||
Vector evaluateError(const LieScalar& vk1, const LieScalar& vk, const LieScalar& q,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none,
|
||||
|
|
@ -99,10 +101,123 @@ public:
|
|||
const size_t p = LieScalar::Dim();
|
||||
if (H1) *H1 = -eye(p);
|
||||
if (H2) *H2 = eye(p);
|
||||
if (H3) *H3 = -eye(p)*dt_*g_/L_*cos(q.value());
|
||||
return vk1.localCoordinates(LieScalar(vk - dt_*g_/L_*sin(q)));
|
||||
if (H3) *H3 = -eye(p)*h_*g_/r_*cos(q.value());
|
||||
return vk1.localCoordinates(LieScalar(vk - h_*g_/r_*sin(q)));
|
||||
}
|
||||
|
||||
}; // \PendulumFactor2
|
||||
|
||||
|
||||
//*************************************************************************
|
||||
/**
|
||||
* This class implements the first position-momentum update rule
|
||||
* p_k = -D_1 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)+mgrh(1-\alpha)\,\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right)
|
||||
* = (1/h)mr^2 (q_{k+1}-q_k) + mgrh(1-alpha) sin ((1-alpha)q_k+\alpha q_{k+1})
|
||||
*/
|
||||
class PendulumFactorPk: public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> {
|
||||
public:
|
||||
|
||||
protected:
|
||||
typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base;
|
||||
|
||||
/** default constructor to allow for serialization */
|
||||
PendulumFactorPk() {}
|
||||
|
||||
double h_; //! time step
|
||||
double m_; //! mass
|
||||
double r_; //! length
|
||||
double g_; //! gravity
|
||||
double alpha_; //! in [0,1], define the mid-point between [q_k,q_{k+1}] for approximation. The sympletic rule above can be obtained as a special case when alpha = 0.
|
||||
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<PendulumFactorPk > shared_ptr;
|
||||
|
||||
///Constructor
|
||||
PendulumFactorPk(Key pKey, Key qKey, Key qKey1,
|
||||
double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0)
|
||||
: Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), pKey, qKey, qKey1),
|
||||
h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
|
||||
|
||||
/// @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 PendulumFactorPk(*this))); }
|
||||
|
||||
/** 1/h mr^2 (qk1-qk)+mgrh (1-a) sin((1-a)pk + a*pk1) - pk = 0, with optional derivatives */
|
||||
Vector evaluateError(const LieScalar& pk, const LieScalar& qk, const LieScalar& qk1,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none,
|
||||
boost::optional<Matrix&> H3 = boost::none) const {
|
||||
const size_t p = LieScalar::Dim();
|
||||
|
||||
double qmid = (1-alpha_)*qk + alpha_*qk1;
|
||||
double mr2_h = 1/h_*m_*r_*r_;
|
||||
double mgrh = m_*g_*r_*h_;
|
||||
|
||||
if (H1) *H1 = -eye(p);
|
||||
if (H2) *H2 = eye(p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid));
|
||||
if (H3) *H3 = eye(p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid));
|
||||
|
||||
return pk.localCoordinates(LieScalar(mr2_h*(qk1-qk) + mgrh*(1-alpha_)*sin(qmid)));
|
||||
}
|
||||
|
||||
}; // \PendulumFactorPk
|
||||
|
||||
//*************************************************************************
|
||||
/**
|
||||
* This class implements the second position-momentum update rule
|
||||
* p_k1 = D_2 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)-mgrh\alpha\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right)
|
||||
* = (1/h)mr^2 (q_{k+1}-q_k) - mgrh alpha sin ((1-alpha)q_k+\alpha q_{k+1})
|
||||
*/
|
||||
class PendulumFactorPk1: public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> {
|
||||
public:
|
||||
|
||||
protected:
|
||||
typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base;
|
||||
|
||||
/** default constructor to allow for serialization */
|
||||
PendulumFactorPk1() {}
|
||||
|
||||
double h_; //! time step
|
||||
double m_; //! mass
|
||||
double r_; //! length
|
||||
double g_; //! gravity
|
||||
double alpha_; //! in [0,1], define the mid-point between [q_k,q_{k+1}] for approximation. The sympletic rule above can be obtained as a special case when alpha = 0.
|
||||
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<PendulumFactorPk1 > shared_ptr;
|
||||
|
||||
///Constructor
|
||||
PendulumFactorPk1(Key pKey1, Key qKey, Key qKey1,
|
||||
double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0)
|
||||
: Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), pKey1, qKey, qKey1),
|
||||
h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
|
||||
|
||||
/// @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 PendulumFactorPk1(*this))); }
|
||||
|
||||
/** 1/h mr^2 (qk1-qk) - mgrh a sin((1-a)pk + a*pk1) - pk1 = 0, with optional derivatives */
|
||||
Vector evaluateError(const LieScalar& pk1, const LieScalar& qk, const LieScalar& qk1,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none,
|
||||
boost::optional<Matrix&> H3 = boost::none) const {
|
||||
const size_t p = LieScalar::Dim();
|
||||
|
||||
double qmid = (1-alpha_)*qk + alpha_*qk1;
|
||||
double mr2_h = 1/h_*m_*r_*r_;
|
||||
double mgrh = m_*g_*r_*h_;
|
||||
|
||||
if (H1) *H1 = -eye(p);
|
||||
if (H2) *H2 = eye(p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid));
|
||||
if (H3) *H3 = eye(p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid));
|
||||
|
||||
return pk1.localCoordinates(LieScalar(mr2_h*(qk1-qk) - mgrh*alpha_*sin(qmid)));
|
||||
}
|
||||
|
||||
}; // \PendulumFactorPk1
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -12,17 +12,17 @@ using namespace gtsam;
|
|||
using namespace gtsam::symbol_shorthand;
|
||||
|
||||
const double tol=1e-5;
|
||||
const double dt = 0.1;
|
||||
const double h = 0.1;
|
||||
const double g = 9.81, l = 1.0;
|
||||
|
||||
const double deg2rad = M_PI/180.0;
|
||||
LieScalar origin, q1(deg2rad*30.0), q2(deg2rad*31.0);
|
||||
LieScalar v1(deg2rad*1.0/dt), v2((v1-dt*g/l*sin(q1)));
|
||||
LieScalar v1(deg2rad*1.0/h), v2((v1-h*g/l*sin(q1)));
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testPendulumFactor1, evaluateError) {
|
||||
// hard constraints don't need a noise model
|
||||
PendulumFactor1 constraint(Q(2), Q(1), V(1), dt);
|
||||
PendulumFactor1 constraint(Q(2), Q(1), V(1), h);
|
||||
|
||||
// verify error function
|
||||
EXPECT(assert_equal(zero(1), constraint.evaluateError(q2, q1, v1), tol));
|
||||
|
|
@ -31,12 +31,34 @@ TEST( testPendulumFactor1, evaluateError) {
|
|||
/* ************************************************************************* */
|
||||
TEST( testPendulumFactor2, evaluateError) {
|
||||
// hard constraints don't need a noise model
|
||||
PendulumFactor2 constraint(V(2), V(1), Q(1), dt);
|
||||
PendulumFactor2 constraint(V(2), V(1), Q(1), h);
|
||||
|
||||
// verify error function
|
||||
EXPECT(assert_equal(zero(1), constraint.evaluateError(v2, v1, q1), tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testPendulumFactorPk, evaluateError) {
|
||||
// hard constraints don't need a noise model
|
||||
PendulumFactorPk constraint(P(1), Q(1), Q(2), h);
|
||||
|
||||
LieScalar pk( 1/h * (q2-q1) + h*g*sin(q1) );
|
||||
|
||||
// verify error function
|
||||
EXPECT(assert_equal(zero(1), constraint.evaluateError(pk, q1, q2), tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testPendulumFactorPk1, evaluateError) {
|
||||
// hard constraints don't need a noise model
|
||||
PendulumFactorPk1 constraint(P(2), Q(1), Q(2), h);
|
||||
|
||||
LieScalar pk1( 1/h * (q2-q1) );
|
||||
|
||||
// verify error function
|
||||
EXPECT(assert_equal(zero(1), constraint.evaluateError(pk1, q1, q2), tol));
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
|
|
|
|||
|
|
@ -398,6 +398,21 @@ virtual class PendulumFactor2 : gtsam::NonlinearFactor {
|
|||
Vector evaluateError(const gtsam::LieScalar& vk1, const gtsam::LieScalar& vk, const gtsam::LieScalar& q) const;
|
||||
};
|
||||
|
||||
virtual class PendulumFactorPk : gtsam::NonlinearFactor {
|
||||
/** Standard constructor */
|
||||
PendulumFactorPk(size_t pk, size_t qk, size_t qk1, double h, double m, double r, double g, double alpha);
|
||||
|
||||
Vector evaluateError(const gtsam::LieScalar& pk, const gtsam::LieScalar& qk, const gtsam::LieScalar& qk1) const;
|
||||
};
|
||||
|
||||
virtual class PendulumFactorPk1 : gtsam::NonlinearFactor {
|
||||
/** Standard constructor */
|
||||
PendulumFactorPk1(size_t pk1, size_t qk, size_t qk1, double h, double m, double r, double g, double alpha);
|
||||
|
||||
Vector evaluateError(const gtsam::LieScalar& pk1, const gtsam::LieScalar& qk, const gtsam::LieScalar& qk1) const;
|
||||
};
|
||||
|
||||
|
||||
//*************************************************************************
|
||||
// nonlinear
|
||||
//*************************************************************************
|
||||
|
|
|
|||
Loading…
Reference in New Issue