SinglePendulum.java

/*
 * $Id: SinglePendulum.java,v 1.3 2008/06/26 10:10:34 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.system.continuous.DoubleBaseContinuousExplicitDynamicSystem;
import org.mklab.tool.control.system.parameter.Parameter;
import org.mklab.tool.control.system.parameter.QuantityType;
import org.mklab.tool.control.system.parameter.SIunit;


/**
 * 1重倒立振子を表すクラスです。
 * 
 * @author koga
 * @version $Revision: 1.3 $, 2004/05/31
 */
public class SinglePendulum extends DoubleBaseContinuousExplicitDynamicSystem {

  /** 台車の質量 */
  @Parameter(name = "M", unit = SIunit.kg, description = "台車の質量")
  protected double m1 = 0.16;

  /** 振子の質量 */
  @Parameter(name = "m", unit = SIunit.kg, description = "振子の質量")
  protected double m2 = 0.039;

  /** 振子の慣性モーメント */
  @Parameter(name = "J", quantity = QuantityType.MOMENT_OF_INERTIA, description = "振子の慣性モーメント")
  protected double jj = 4.485e-4;

  /** 振子の長さ */
  @Parameter(name = "l", unit = SIunit.m, description = "振子の長さ")
  protected double len = 0.121;

  /** 台車の摩擦係数 */
  @Parameter(name = "Fr", unit = {SIunit.N, SIunit.m_1, SIunit.s}, description = "台車の摩擦係数")
  protected double fr = 2.6;

  /** 振子の回転摩擦係数 */
  @Parameter(name = "Cr", unit = {SIunit.N, SIunit.m, SIunit.rad_1, SIunit.s}, description = "振子の回転摩擦係数")
  protected double cr = 4.210e-4;

  /** 電圧トルク変換係数 */
  @Parameter(name = "a", unit = {SIunit.m, SIunit.N, SIunit.V_1}, description = "電圧トルク変換係数")
  protected double a0 = 0.1;

  /** 台車位置測定センサーの出力を台車位置に変換する係数 */
  @Parameter(name = "cc1", unit = {SIunit.m, SIunit.V_1}, description = "台車の位置センサーの変換係数")
  protected double cc1 = 1.0;

  /** 振子角度測定センサーの出力を振子角度に変換する係数 */
  @Parameter(name = "cc2", unit = {SIunit.rad, SIunit.V_1}, description = "振子の角度センサーの変換係数")
  protected double cc2 = 1.0;

  /** 重力加速度 */
  final protected double g = 9.8;

  /**
   * コンストラクタ
   */
  public SinglePendulum() {
    super(1, 2, 4); // 1入力、2出力、4状態
    setHasDirectFeedthrough(false); // 直達項無し
  }

  /**
   * {@inheritDoc}
   */
  public DoubleMatrix stateEquation( final double t, final DoubleMatrix x, final DoubleMatrix u) {
    final double th = x.getDoubleElement(2);
    final double dr = x.getDoubleElement(3);
    final double dth = x.getDoubleElement(4);
    final double u1 = u.getDoubleElement(1);

    final double cos = Math.cos(th);
    final double sin = Math.sin(th);
    final DoubleMatrix massMatrix = new DoubleMatrix(new double[][] { {this.m1 + this.m2, this.m2 * this.len * cos}, {this.m2 * this.len * cos, this.jj + this.m2 * this.len * this.len}});
    final DoubleMatrix forceVector = new DoubleMatrix(new double[][] { {-this.fr * dr + this.m2 * this.len * sin * dth * dth + this.a0 * u1}, {this.m2 * this.g * this.len * sin - this.cr * dth}});

    final DoubleMatrix ddrth = massMatrix.leftDivide(forceVector);

    final DoubleMatrix dx = new DoubleMatrix(4, 1);
    dx.setElement(1, dr);
    dx.setElement(2, dth);
    dx.setElement(3, ddrth.getDoubleElement(1, 1));
    dx.setElement(4, ddrth.getDoubleElement(2, 1));
    return dx;
  }

  /**
   * {@inheritDoc}
   */
  @Override
  public DoubleMatrix outputEquation( final double t, final DoubleMatrix x) {
    final double r = x.getDoubleElement(1);
    final double th = x.getDoubleElement(2);

    final DoubleMatrix y = new DoubleMatrix(2, 1);
    y.setElement(1, this.cc1 * r);
    y.setElement(2, this.cc2 * th);
    return y;
  }
}