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

}