Pendulum.h

Go to the documentation of this file.

 
#pragma once
 
#include <gtsam/nonlinear/NonlinearFactor.h>
 
namespace gtsam {
 
//*************************************************************************
class PendulumFactor1: public NoiseModelFactor3<double, double, double> {
public:
 
protected:
  typedef NoiseModelFactor3<double, double, double> Base;
 
  PendulumFactor1() {}
 
  double h_;  // time step
 
public:
 
  typedef boost::shared_ptr<PendulumFactor1> shared_ptr;
 
  PendulumFactor1(Key k1, Key k, Key velKey, double h, double mu = 1000.0)
  : Base(noiseModel::Constrained::All(1, std::abs(mu)), k1, k, velKey), h_(h) {}
 
  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new PendulumFactor1(*this))); }
 
  Vector evaluateError(const double& qk1, const double& qk, const double& v,
      boost::optional<Matrix&> H1 = boost::none,
      boost::optional<Matrix&> H2 = boost::none,
      boost::optional<Matrix&> H3 = boost::none) const override {
    const size_t p = 1;
    if (H1) *H1 = -Matrix::Identity(p,p);
    if (H2) *H2 = Matrix::Identity(p,p);
    if (H3) *H3 = Matrix::Identity(p,p)*h_;
    return (Vector(1) << qk+v*h_-qk1).finished();
  }
 
}; // \PendulumFactor1
 
 
//*************************************************************************
class PendulumFactor2: public NoiseModelFactor3<double, double, double> {
public:
 
protected:
  typedef NoiseModelFactor3<double, double, double> Base;
 
  PendulumFactor2() {}
 
  double h_;
  double g_;
  double r_;
 
public:
 
  typedef boost::shared_ptr<PendulumFactor2 > shared_ptr;
 
  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(1, std::abs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {}
 
  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new PendulumFactor2(*this))); }
 
  Vector evaluateError(const double & vk1, const double & vk, const double & q,
      boost::optional<Matrix&> H1 = boost::none,
      boost::optional<Matrix&> H2 = boost::none,
      boost::optional<Matrix&> H3 = boost::none) const override {
    const size_t p = 1;
    if (H1) *H1 = -Matrix::Identity(p,p);
    if (H2) *H2 = Matrix::Identity(p,p);
    if (H3) *H3 = -Matrix::Identity(p,p)*h_*g_/r_*cos(q);
    return (Vector(1) << vk - h_ * g_ / r_ * sin(q) - vk1).finished();
  }
 
}; // \PendulumFactor2
 
 
//*************************************************************************
class PendulumFactorPk: public NoiseModelFactor3<double, double, double> {
public:
 
protected:
  typedef NoiseModelFactor3<double, double, double> Base;
 
  PendulumFactorPk() {}
 
  double h_;  
  double m_;  
  double r_;  
  double g_;  
  double alpha_; 
 
public:
 
  typedef boost::shared_ptr<PendulumFactorPk > shared_ptr;
 
  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(1, std::abs(mu)), pKey, qKey, qKey1),
    h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
 
  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk(*this))); }
 
  Vector evaluateError(const double & pk, const double & qk, const double & qk1,
      boost::optional<Matrix&> H1 = boost::none,
      boost::optional<Matrix&> H2 = boost::none,
      boost::optional<Matrix&> H3 = boost::none) const override {
    const size_t p = 1;
 
    double qmid = (1-alpha_)*qk + alpha_*qk1;
    double mr2_h = 1/h_*m_*r_*r_;
    double mgrh  = m_*g_*r_*h_;
 
    if (H1) *H1 = -Matrix::Identity(p,p);
    if (H2) *H2 = Matrix::Identity(p,p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid));
    if (H3) *H3 = Matrix::Identity(p,p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid));
 
    return (Vector(1) << mr2_h * (qk1 - qk) + mgrh * (1 - alpha_) * sin(qmid) - pk).finished();
  }
 
}; // \PendulumFactorPk
 
//*************************************************************************
class PendulumFactorPk1: public NoiseModelFactor3<double, double, double> {
public:
 
protected:
  typedef NoiseModelFactor3<double, double, double> Base;
 
  PendulumFactorPk1() {}
 
  double h_;  
  double m_;  
  double r_;  
  double g_;  
  double alpha_; 
 
public:
 
  typedef boost::shared_ptr<PendulumFactorPk1 > shared_ptr;
 
  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(1, std::abs(mu)), pKey1, qKey, qKey1),
    h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
 
  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk1(*this))); }
 
  Vector evaluateError(const double & pk1, const double & qk, const double & qk1,
      boost::optional<Matrix&> H1 = boost::none,
      boost::optional<Matrix&> H2 = boost::none,
      boost::optional<Matrix&> H3 = boost::none) const override {
    const size_t p = 1;
 
    double qmid = (1-alpha_)*qk + alpha_*qk1;
    double mr2_h = 1/h_*m_*r_*r_;
    double mgrh  = m_*g_*r_*h_;
 
    if (H1) *H1 = -Matrix::Identity(p,p);
    if (H2) *H2 = Matrix::Identity(p,p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid));
    if (H3) *H3 = Matrix::Identity(p,p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid));
 
    return (Vector(1) << mr2_h * (qk1 - qk) - mgrh * alpha_ * sin(qmid) - pk1).finished();
  }
 
}; // \PendulumFactorPk1
 
}