LinearSinglePendulum.java

/*
 * $Id: LinearSinglePendulum.java,v 1.1 2008/03/30 02:16:47 koga Exp $
 *
 * Copyright (C) 2004 Koga Laboratory. All rights reserved.
 *
 */
package org.mklab.tool.control.model.pendulum;

import org.mklab.nfc.matrix.DoubleMatrix;
import org.mklab.tool.control.DoubleLinearSystem;
import org.mklab.tool.control.DoubleLinearSystemFactory;
import org.mklab.tool.control.system.continuous.DoubleContinuousLinearDynamicSystem;


/**
 * (1重)倒立振子の線形近似モデルを表すクラスです。
 * 
 * @author koga
 * @version $Revision: 1.1 $, 2004/05/31
 */
public class LinearSinglePendulum extends DoubleContinuousLinearDynamicSystem {

  /** 非線形倒立振子 */
  private SinglePendulum pendulum;

  /**
   * 新しく生成された<code>LinearSinglePendulum</code>オブジェクトを初期化します。
   * 
   * @param pendulum (1重)倒立振子
   */
  public LinearSinglePendulum(final SinglePendulum pendulum) {
    super();

    this.pendulum = pendulum;

    final DoubleLinearSystem system = createLinearSystem();
    setLinearSystem(system);

    setInputSize(system.getInputSize());
    setOutputSize(system.getOutputSize());
    setStateSize(system.getStateSize());
  }

  /**
   * 新しく生成された<code>LinearSinglePendulum</code>オブジェクトを初期化します。
   */
  public LinearSinglePendulum() {
    this(new SinglePendulum());
  }

  /**
   * (1重)倒立振子モデルを設定します。
   * 
   * @param pendulum (1重)倒立振子モデル
   */
  public void setSinglePendulum(final SinglePendulum pendulum) {
    this.pendulum = pendulum;
    updateParameters();
  }

  /**
   * パラメータを更新します。
   */
  void updateParameters() {
    setLinearSystem(createLinearSystem());
  }

  /**
   * 線形システムを返します。
   * 
   * @return 線形システム
   */
  private DoubleLinearSystem createLinearSystem() {
    final double m1 = this.pendulum.m1;
    final double m2 = this.pendulum.m2;
    final double len = this.pendulum.len;
    final double jj = this.pendulum.jj;
    final double cr = this.pendulum.cr;
    final double g = this.pendulum.g;
    final double fr = this.pendulum.fr;
    final double a0 = this.pendulum.a0;

    final double alpha0 = (m1 + m2) * jj + m1 * m2 * len * len;
    double a32 = -(m2 * len) * (m2 * len) * g;
    double a33 = -fr * (jj + m2 * len * len);
    double a34 = m2 * len * cr;
    double a35 = (jj + m2 * len * len) * m2 * len;
    double a42 = (m1 + m2) * m2 * len * g;
    double a43 = m2 * len * fr;
    double a44 = -(m1 + m2) * cr;
    double a45 = -(m2 * len) * (m2 * len);
    double b3 = (jj + m2 * len * len) * a0;
    double b4 = -m2 * len * a0;

    a32 = a32 / alpha0;
    a33 = a33 / alpha0;
    a34 = a34 / alpha0;
    a35 = a35 / alpha0;
    a42 = a42 / alpha0;
    a43 = a43 / alpha0;
    a44 = a44 / alpha0;
    a45 = a45 / alpha0;
    b3 = b3 / alpha0;
    b4 = b4 / alpha0;

    final DoubleMatrix a = new DoubleMatrix(new double[][] { {0, 0, 1, 0}, {0, 0, 0, 1}, {0, a32, a33, a34}, {0, a42, a43, a44}});
    final DoubleMatrix b = new DoubleMatrix(new double[] {0, 0, b3, b4}).transpose();
    final DoubleMatrix c = new DoubleMatrix(new double[][] { {this.pendulum.cc1, 0, 0, 0}, {0, this.pendulum.cc2, 0, 0}});
    final DoubleMatrix d = new DoubleMatrix(2, 1);

    return DoubleLinearSystemFactory.createLinearSystem(a, b, c, d);
  }
}