szmlb.net

tips for robotics

PICOSメモ (整数計画問題)

前回の続き.
PICOSで整数計画問題を解く場合のサンプルコードをメモしておく.

PuLPを使っている以下の記事を参考にPICOSで解いてみる.
qiita.com

ナップサック問題

import picos as pic
import cplex
import scipy as sp
import scipy.linalg as sl
import numpy as np

def testSolveKnapSack():
    """
    ナップサック問題を解く

    min weight * x
    s.t. size * x <= capacity
         x in [0,  1]
    """

    weight = np.array([[22, 12, 16, 10, 35, 26, 42, 53]])
    size = np.array([[21, 11, 15, 9, 34, 25, 41, 52]])
    capacity = 100

    w = pic.new_param('w', weight)
    s = pic.new_param('s', size)

    ks = pic.Problem()

    x = ks.add_variable('x', (size.shape[1], 1),  vtype='binary')
    ks.add_constraint(capacity >= s * x)
    ks.set_objective('max', w * x)

    print(ks)
    ks.solve()
    print(x.value)

順次追加予定

PICOSメモ (半正定値計画問題)

半正定値計画問題(Semi definite programming: SDP)を解きたくてライブラリを探していたところ, この記事にたどり着いた.
記事では, Pythonの最適化モデリングライブラリPICOSを使っていて, 使いやすそうだったので使ってみたくなった.
ユーザーフレンドリーな形で設計されているためより簡単に問題を記述できるらしい.
サンプルは本家のサイトにたくさんあるが, 複雑な例題が多かったのでシンプルな問題を解いてみた.
以下, いくつかサンプルコードをメモしておく.

基本的なコードの書き方

基本的な流れは以下の通り.

1. インポート

import picos as pic
import cvxopt as cvx
import scipy as sp
import scipy.linalg as sl
import numpy as np

2. 係数行列の定義

cnp = np.array([[2,  1]])
c = pic.new_param('c', cnp)

3. picosインスタンスの生成

sdp = pic.Problem()

4. 説明変数の定義

x = sdp.add_variable('x', (cnp.shape[1], 1))

5. 制約の記述

sdp.add_constraint(-x[0] + x[1] <= 1.0)
sdp.add_constraint(x[0] + x[1] >= 2.0)
sdp.add_constraint(x[1] >= 0.0)
sdp.add_constraint(x[0] - 2.0 * x[1] <= 4.0)

6. 目的関数の記述

sdp.set_objective('min', c*x)

7. 解く

sdp.solve()

線形計画問題 (Linear programming: LP)を解く

SDPは, LPの対称行列空間への拡張と考えることができるので, 最適化問題のクラスとしては, LPを内包した形になっている. よって, LPも問題なく解ける (ライブラリの処理としてはLPのソルバを呼んでいるだけ?)

import picos as pic
import cvxopt as cvx
import scipy as sp
import scipy.linalg as sl
import numpy as np

def testSolveLP():
    """
    線形計画問題を解く
    CVXOPTの線形計画問題のサンプル:

    min 2.0 * x1 +  1.0 * x2
    s.t. -x1 + x2 <= 1.0
         x1 + x2 >= 2.0
         x2 >= 0.0
         x1 - 2.0 * x2 <= 4.0
    """
    cnp = np.array([[2,  1]])
    c = pic.new_param('c', cnp)

    sdp = pic.Problem()

    x = sdp.add_variable('x', (cnp.shape[1], 1))
    sdp.add_constraint(-x[0] + x[1] <= 1.0)
    sdp.add_constraint(x[0] + x[1] >= 2.0)
    sdp.add_constraint(x[1] >= 0.0)
    sdp.add_constraint(x[0] - 2.0 * x[1] <= 4.0)

    sdp.set_objective('min', c*x)

    print(sdp)
    sdp.solve()
    print(x.value)

二次計画問題 (Quadratic programming: QP)を解く

SDPはQPも内包している. 例えば以下のQPの標準形をSDPに変換することを考える.

f:id:szmlb:20180101225006j:plain
QPの目的関数と制約条件

{\bf Q}が正定行列であれば, その平方根行列を求めることができ, \frac{1}{2}{\bf Q} = {\bf M}^T{\bf M}のように分解できる. さらにシュールの補題を駆使して問題を変形すると, 以下のようになる.

f:id:szmlb:20180101230629j:plain
QPの標準形と等価なSDPの問題設定

問題の式変形は以下の書籍を参考にした.

システム制御のための最適化理論 (システム制御工学シリーズ)

システム制御のための最適化理論 (システム制御工学シリーズ)

上記の問題を, PICOSでは以下のように簡潔に記述できる.

def testSolveQP():
    """
    二次計画問題をSDP形式でわざわざ解く
    CVXOPTの二次計画問題のサンプル:

    min 2.0 * x1**2 + x2**2 + x1 * x2 + x1 + x2
    s.t. x1 >= 0.0
         x2 >= 0.0
         x1 + x2 = 1.0
    """

    Anp = np.array([[4, 1],
                    [1, 2]])
    bnp = np.array([[1], [1]])

    Mnp = sl.sqrtm(Anp/2.0) # 行列Anpの平方根

    b = pic.new_param('b', bnp)
    M = pic.new_param('M', Mnp)
    I = pic.new_param('I', sp.eye(bnp.shape[0]) )

    sdp = pic.Problem()
    f0 = sdp.add_variable('f0', (1, 1))
    x = sdp.add_variable('x', (bnp.shape[0], 1))
    sdp.add_constraint((( f0 - b.T*x & (M*x).T ) // (M*x & I))>>0 ) # シュールの補題より
    sdp.add_constraint(x[0] >= 0.0)
    sdp.add_constraint(x[1] >= 0.0)
    sdp.add_constraint(x[0] + x[1] == 1.0)

    sdp.set_objective('min', f0)

    print(sdp)
    sdp.solve()
    print(x.value)

最小固有値問題を解く

任意の行列の固有値のなかで最小のものを求める問題を, PICOSで解く.
最小固有値問題はSDPじゃなくても解けるが, 固有値が全て正になるような制約条件下で特定の関数を最小化(最大化)したいようなケースが, 工学の分野では見受けられる.

最小固有値問題を解くためには, 以下のSDPを解けば良い.

f:id:szmlb:20180101232358j:plain
最小固有値問題の目的関数と制約条件

PICOSで記述すると以下のようになる.

def testSolveMinEigVal():
    """
    最小固有値問題をSDP形式でわざわざ解く:
    A=[4, 1; 1, 2]の最小固有値1.59を求める
    """

    Anp = np.array([[4, 1],
                    [1, 2]])

    A = pic.new_param('A', Anp)
    I = pic.new_param('I', sp.eye(Anp.shape[0]) )

    sdp = pic.Problem()
    mu = sdp.add_variable('mu', (1, 1))
    sdp.add_constraint(A - mu * I >> 0 )
    sdp.set_objective('min', -mu)

    print(sdp)
    sdp.solve()
    print(mu.value)

LMI制約付き問題を解く

最後に一つ制御っぽいサンプルを. LMI制約付き問題を解く.
制御の分野では, ロバスト安定化問題や多目的制御問題をLinear Matrix Inequality: LMIと呼ばれる(半)正定性を制約とした問題設定に落とし込む設計法がよく見受けられる. SDPソルバに投げれば確実に解けるため (BMIと呼ばれるちょっと簡単に解けない問題のクラスもあるが, ここでは言及しない). MatlabにはYALMIPと言う有名なLMIソルバが存在するが, YALMIPのサンプルコードを眺めているとPICOSでも解けそうだったのでポーティングしてみた.

具体的に解いた問題は「LMIによるシステム制御」と言う本の例題4.3.

LMIによるシステム制御 - ロバスト制御系設計のための体系的アプローチ

LMIによるシステム制御 - ロバスト制御系設計のための体系的アプローチ

具体的な問題の内容について興味がある方は, 書籍を読むことをお勧めします.
(ものすごく)ざっくりと説明すると, 制御対象のパラメータ変動がある場合の安定性について判定するために, LMIを解くと言う問題設定になっている. 問題の数式表現は以下のようになる.

f:id:szmlb:20180102000124j:plain
ロバスト安定性判別のためのLMI

上式が解けて, 解が存在すれば安定.
PICOSを使うと以下のように解ける.

def testSolveLyapnovEq():
  """
  "LMIによるシステム制御" p.85の例4.3(ロバスト安定性解析)を解いてみる

  P - I >> 0
  P * A1 + A1.T * P << 0
  P * A2 + A2.T * P << 0
  P * A3 + A3.T * P << 0
  """

  A1a = np.array([[-1.0, 2.9], [1.0, -3.0]])
  A2a = np.array([[-0.8, 1.5], [1.3, -2.7]])
  A3a = np.array([[-1.4, 0.9], [0.7, -2.0]])

  A1 = pic.new_param('A1', A1a )
  A2 = pic.new_param('A2', A2a )
  A3 = pic.new_param('A3', A3a )
  I = pic.new_param('I', sp.eye(2) )

  sdp = pic.Problem()
  Xp = sdp.add_variable('X', (2, 2), vtype='symmetric')
  sdp.add_constraint(Xp-I>>0)
  sdp.add_constraint(Xp*A1+A1.T*Xp << 0)
  sdp.add_constraint(Xp*A2+A2.T*Xp << 0)
  sdp.add_constraint(Xp*A3+A3.T*Xp << 0)

  sdp.set_objective("min", pic.trace(Xp))

  print(sdp)
  sdp.solve()
  print(Xp.value)

gitメモ: gitの使い方

gitに関する昔のメモの整理.

用語の整理
  • ワークツリー : 作業ディレクト
  • ステージ領域(インデックス) : ワークツリーからリポジトリに変更結果をコミットする際の中継ポイント
  • HEAD : ブランチの最新のコミット
  • commit
  • push, pull
  • branch
  • checkout
  • merge
  • rebase
  • cherry_pick
  • fast-forward, non fast-forward
Gitをインストールする

$ sudo apt-get install git-core
$ sudo apt-get install gitk // GUIでGitを管理

作業ディレクトリをGit管理下のディレクトリに指定

$ git init

Git管理下のディレクトリの状態を表示

$ git status

# On branch master
#
# Initial commit
#
# Untracked files:
# (use "git add ..." to include in what will be committed)
#
# Makefile
# main.cpp

ここで, 未管理のファイルMakefile, main.cppが"Untracked files"という表現で列挙されています.

管理したいファイルを指定, 追加

Untracked filesを, 管理対象のファイルに変更します.

個別指定
$ git add "file name"

一括指定
$ git add ./*

全てのファイルを管理対象に指定すると,

# On branch master
#
# Initial commit
#
# Changes to be committed:
# (use "git rm --cached ..." to unstage)
#
# new file: Makefile
# new file: main.cpp
# new file: readme.txt
#

と表示されます.
Makefile, main.cpp, readme.txtという3つのファイルが管理下に置かれました.

コミット

コミットします.
コミットすることで, addしたファイルが新規変更分として保存されます.
その際, コメントを残しておくことでどのような変更が行われたか, ということを明確にする事ができます.

$git commit

以下のような編集画面が現れるので, 上部にコメントを書いて編集を終了します.

First commit for Preview Control program.
# Please enter the commit message for your changes. Lines starting
# with '#' will be ignored, and an empty message aborts the commit.
#
# Committer: szmlb
#
# On branch master
# Changes to be committed:
# (use "git reset HEAD ..." to unstage)
#
# modified: readme.txt
#

ログを表示する

$ git log

commit: *****

Author: szmlb
Date: Mon Dec 31 16:39:34 2012 +0900

First commit for Preview Controller program.

commit: 後の **** がコミット名になり, commit時のコメントが同時に表示されていることが分かります.

詳細を確認したい場合は,

$ git show ********

として確認します.

$ git show

とだけ打ち込むと, マスターブランチのtailのコミット詳細が表示されます.

コミット名を省略する

毎回 $git show ******** のように指定するのは面倒です.
そこで, 代替のタグ名を指定し, 入力を簡単化します.

$ git tag ver1 ********
$ git show ver1

で詳細を表示可能.

コミット間の差分を表示する

最低限使うdiffの機能がまとまっている.
よく使う git diff コマンド - Qiita

インデックスとワークツリーの差分を表示 (addしていないファイルを表示)
$ git diff

インデックスとHEADの差分を表示
$ git diff --cached

直前のコミットによる変更の表示
$ git diff HEAD^ HEAD

ブランチを切る

本流と支流を作ります.
本流でメインの作業を続け, 支流では別の作業を行うことができます.
また, あとで本流と支流を合流(マージする)させることもできます.

ブランチを確認する

$git branch
* master

ブランチを作成する

$git branch recursive
$git branch
* master
recursive

"recursive"という名前のブランチを作成しました.

ブランチを切り替える

作業を行うブランチを切り替えます.
現在,

* master
recursive

の二種類のブランチが存在します.

$git checkout recursive

によって, recursiveブランチに切り替えます.
次に, readme.txtを変更し, commitします.
commit後, 再度マスターブランチに戻って修正に影響があるかどうかを確認すると,
変化がないことがわかります.

ブランチをマージする
  • 1つブランチを切っていた場合のマージ

以下のツリーの状態を考える.

---> *master ---> branch1

masterブランチでbranch1の内容を統合する.

$ git merge branch1

すると, ツリーの構造は以下のようになる.

--->・---> *master, branch1

となる.

branch1は不要なので, 以下のコマンドで削除する.

$ git branch -d branch1

--->・---> *master

このように, 一度作成したブランチの情報を消して1つのブランチにまとめることをfast-forward マージと言う.

  • 2つ以上ブランチを切っていた場合のマージ

以下のツリーの状態を考える.

--- > *master ---> branch1
     \
      ---> brahch2

2つのブランチを順にマージする.

$ git merge branch1

--- > ・ ---> *master, branch1
    \
     ---> brahch2

$ git merge branch2

masterの内容(branch1の内容をマージ)と, branch2の内容をマージしようとすると, コンフリクトが発生する.
(branch2は, branch1をマージする前のmasterをベースにしているため)
この時点で, コンフリクトが発生しているソースコードにgitが差分を加えているので, それを自力で修正する.

最後に, 修正をaddしてコミットすると,

$ git commit

以下のようなツリー構造になる.

--- > ・ ---> branch1 ---> *master
    \         /
     ---> brahch2 ---

この時はマージの履歴が残り, non fast-forward マージと呼ばれる.

  • リベースでマージする

mergeコマンドではなく, rebaseコマンドでマージすることもできる.
branch1をマージした状態の以下のツリー構造を考える.
branch2にチェックアウトしておく.

--- > ・ ---> master, branch1
    \
     ---> *brahch2

$ git rebase master

コンフリクトが生じるので, 修正する.
続いて修正をaddする.
rebaseの場合はコミットせずに以下のコマンドを叩く.

$git rebase --continue

--- > ・ ---> master, branch1 ---> *brahch2

最後に, masterにチェックアウトしてbranch2をマージする.

$ git checkout master
$ git merge branch2

--- > ・ ---> branch1 ---> *master, brahch2

ブランチとは【ブランチ】 | サルでもわかるGit入門 〜バージョン管理を使いこなそう〜 | どこでもプロジェクト管理バックログ

以前のコミットまで状態を戻す
  • checkoutで戻る (要検討)

$ git checkout <コミットID>

昔のコミットに戻ると, 戻ったコミット以降のコミットがgit log等で見えなくなる.

$git branch で確認すると,

* (detached from 0b4711f)
master
hoge
hogeee

のように一時的なブランチが切られているので,

$git checkout master で戻ればOK.

Git で過去の Commit に一時的に戻り、作業後、また最新のCommitを復活させる - Qiita

  • 現在のコミットを削除して以前のコミットに戻る

$ git reset --hard <コミットID>
Git:以前のコミットまで戻す方法 - tyoshikawa1106のブログ
Git 前のバージョンに戻すには? - code snippets

rebaseでcommit情報を変更する

commitのログを変更したり, commitを削除するには rebase が使える.
下記のサイトが参考になる.

参考

ubuntuにrt-preemptカーネルを導入する

基本的に以下のサイトを参考にして導入した.
RT PREEMPT HOWTO - RTwiki

環境

$lsb_release -r
Release 14.04

$uname -r
3.13.0-66-generic

$cat /proc/cpuinfo | grep "model name"
model name : Intel(R) Core(TM) i3-4170 CPU @ 3.70GHz

パッチを当てる

$ zcat ../patches-3.12.66-rt88.patch.gz | patch -p1

エラーが出ず "patching..." のように表示されればOK.

カーネルのconfigure

$ make menuconfig

RT PREEMPT HOWTOのページには,

・enable CONFIG_PREEMPT_RT
・activated the High-Resolution-Timer Option
・disabled all Power Management Options like ACPI or APM (Since rt patch 2.6.18-rt6 you will probably have to activate ACPI option to activate high resolution timer.)

と書かれているので, 以下のように設定.

Kernel Features
---> Preemption Model (Fully Preemptible Kernel (RT))
---> [*] Fully Preemptible Kernel (RT)

General setup
---> Timers subsystem
---> [*] High Resolution Timer Support **デフォルトでチェック済み

Power management and ACPI options
---> [*] ACPI (Advanced Configuration and Power Interface) Support **>2.6.18の場合はチェック

Kernel hacking
---> Memory Debugging
---> [ ] Check for stack overflows #Already deselected - do not select

あとは, RTAIでの設定についても反映. これは要るかどうかわかりません.
RTAI installation for ubuntu 14.04 · GitHub

Processor type and features
---> Processor family = Select yours **Generic-x86-64に設定

---> Maximum number of CPUs (NR_CPUS) = Set your number **4に設定

---> [ ] SMT (Hyperthreading) scheduler support = DISABLE IT **チェックを外す

ビルド

$ sudo make
$ sudo make modules_install
$ sudo make install
$ sudo grub-update
$ sudo reboot

3.12.66-rt88がgrubの画面に出ていればOK.

結果

アナログ出力波形をオシロで確認しましたが, low-latencyカーネルと比較してlatencyが低減できています.
RTAIやRT-Linuxよりは導入が簡単で, low-latencyカーネルより精度が高いのでオススメです.
(rtカーネルとrt-preemptカーネルの比較はできていません)

FTDI USBドライバを利用したLinuxでのシリアル通信メモ

Linuxでシリアル通信のプログラムを作成する際のメモ.
PythonのpySerialライブラリや, QtのQtSerialPortクラス, ROSではrosserialがあるので需要は少ないかもしれないが, C/C++FTDIを利用してシリアル通信する必要があったのでメモを残しておく.

C/C++でのシリアル通信プログラミング

最近のLinuxディストリビューションでは, シリアル通信のためのドライバFTDIがデフォルトで組み込まれており利用が可能である.
FTDIを利用したC/C++用の解説サイトとして下記のサイトが非常に分かりやすい.
xanthium.in

また, サンプルコードがGitHubで公開されている.
github.com

ボーレートのカスタム設定

通常のFTDI用APIでは, ボーレートの設定に不都合があるらしい.
例えば307.2kbpsのボーレートに設定しようとするとコンパイルの段階でエラーとなる.
その点に関して下記のサイトで議論されており, また解決策も提示されている.
stackoverflow.com

対策用のソースコードココ.

サンプルコード

以上のコードを参考にしてゴテゴテくっつけただけのプログラムは以下の通りです.
ポートを開いて書き込んで読むだけです.
github.com

参考

qpOASESメモ

二次計画問題を解くためのライブラリ.
https://projects.coin-or.org/qpOASES

  • インストール

https://projects.coin-or.org/qpOASES/wiki/QpoasesInstallation

インストールから使用までの手引きは以下のpdfファイルに記載してある.
http://www.coin-or.org/qpOASES/doc/3.2/manual.pdf

zipファイルをDLして,

$ zip qpOASES-3.2.0.zip
$ cd qpOASES-3.2.0
$ vim make.mk

使用するOSに準じて変更する↓(Macを使用する例)
#include ${TOP}/make_linux.mk
#include ${TOP}/make_cygwin.mk
#include ${TOP}/make_windows.mk
include ${TOP}/make_osx.mk

次に, 対応するmkファイルを編集する.
$ vim make_osx.mk

例えば, XcodeMatlabのパスを自分の環境に合わせて変更する必要がある.

################################################################################
# user configuration

# include directories, relative
IDIR =   ${TOP}/include
SRCDIR = ${TOP}/src
BINDIR = ${TOP}/bin

# MacOSX SDK
SYSROOT = /Applications/Xcode.app/Contents/Developer/Platforms/MacOSX.platform/Developer/SDKs/<b>MacOSX10.11</b>.sdk
SDK = -isysroot ${SYSROOT} -stdlib=libc++

# Matlab include directory (ADAPT TO YOUR LOCAL SETTINGS!)
MATLAB_IDIR   = /Applications/<b>MATLAB_R2015b.app</b>/extern/include/
MATLAB_LIBDIR =

最後に,

$ make

これでOK.

自分で使用するプロジェクト内で使用するために, 手動でincludeファイルとlinkファイルを以下のようにコピーした.

$ sudo cp ./include/* /usr/local/include
$ sudo cp ./bin/libqpOASES.* /usr/local/lib

  • 実行例

以下の2次計画問題を解く.
http://cvxopt.org/_images/math/7f776871eb1407c7fecdb53e4be9fc3e3826668e.png
Solving a quadratic program — CVXOPT
問題はcvxoptのドキュメントから拝借しました.
解は, (x1, x2)=(0.25, 0.75)です.

コードは以下の通りです.
github.com

#include <qpOASES.hpp>

int main( )
{
    USING_NAMESPACE_QPOASES

    /* Setup data of QP. */
    real_t H[2*2] = { 4.0, 1.0, 1.0, 2.0 };
    real_t A[2*1] = { 1.0, 1.0 };
    real_t g[2] = { 1.0, 1.0 };
    real_t lb[2] = { 0.0, 0.0 };
    real_t ub[2] = { 10000.0, 10000.0 };
    real_t lbA[1] = { 1.0 }; // lbAとubAを等しくすることで等式制約として設定可能
    real_t ubA[1] = { 1.0 };

    /* Setting up QProblem object. */
    QProblem example( 2,1 );

    Options options;
    example.setOptions( options );

    /* Solve first QP. */
    int_t nWSR = 10;
    example.init( H,g,A,lb,ub,lbA,ubA, nWSR );

    /* Get and print solution of first QP. */
    real_t xOpt[2];
    real_t yOpt[2+1];
    example.getPrimalSolution( xOpt );
    example.getDualSolution( yOpt );
    printf( "\nxOpt = [ %e, %e ];  yOpt = [ %e, %e, %e ];  objVal = %e\n\n",
            xOpt[0],xOpt[1],yOpt[0],yOpt[1],yOpt[2],example.getObjVal() );

    example.printOptions();
    example.printProperties();

    return 0;
}

Makefileの中で以下のようにリンクすればOK.

CPP = clang++
CFRAGS = -stdlib=libstdc++ -O3 -Wall -lm -g
QPOASES_INCLUDE_DIR = -I/usr/local/include/
QPOASES_LIB_DIR = -L/usr/local/lib -lqpOASES

OBJS_MAIN = main.o

main : $(OBJS_MAIN)
	$(CPP) -o main $(CFLAGS) $(QPOASES_INCLUDE_DIR) $(QPOASES_LIB_DIR) $(OBJS_MAIN)

# main program
main.o: main.cpp
		$(CPP) -c $(CFLAGS) $(QPOASES_INCLUDE_DIR) $(QPOASES_LIB_DIR) main.cpp


ODEで弾性関節を作成する

(過去ブログの転載)

最近ODEでロボットを作成しては制御する, という作業を繰り返しています. 今回は, RHexライク(とい
うかそのもの)をつくってみました.

youtu.be

脚部が弾性のある素材で構成されていますので, ODEのBoxを弾性関節でつないで実現します.

下記サイトでは, 関節に弾性を持たせて連結し, 蛇ロボットを実現しています.

ODEのジョイントで角度ばねを実現する
http://ktaobo.blogspot.jp/2013/05/ode-damper-spring.html

作成した結果, 上記の画像のようになりました.

各脚は9個のBoxで構成しています.

Base Bodyと連結するjointだけ通常のHingeJointとし, それ以外のJointには上記サイトの例のよう

に弾性と粘性を設定して, 駆動範囲を制限しています.

jointの設定部のコードは以下の通りです.

dReal angle1 = 0.0;
dReal h = dt;  // タイムステップ
dReal kp = 40;  // バネ係数
dReal kd = 1.0;  // ダンパー係数
dReal erp = h * kp / (h * kp + kd);
dReal cfm = 1.0 / (h * kp + kd);
for(int i = 0; i < 9; i++){
    joint[i] = dJointCreateHinge(world, 0); // ヒンジ関節生成
    if(i == 0){
        dJointAttach(joint[i], base_robot[0].body, link_robot[i].body); // 関節の取付け
        dJointSetHingeAnchor(joint[i], anchor_x[i], anchor_y[i], anchor_z[i]); //関節中心の設定
        dJointSetHingeAxis(joint[i], 0, 1, 0); // 関節回転軸の設定
    }
    else{
        dJointAttach(joint[i], link_robot[i-1].body, link_robot[i].body); // 関節の取付け
        dJointSetHingeAnchor(joint[i], anchor_x[i], anchor_y[i], anchor_z[i]); //関節中心の設定
        dJointSetHingeAxis(joint[i], 0, 1, 0); // 関節回転軸の設定
        dJointSetHingeParam(joint[i], dParamLoStop, angle1);
        dJointSetHingeParam(joint[i], dParamHiStop, angle1);
        dJointSetHingeParam(joint[i], dParamStopERP, erp);
        dJointSetHingeParam(joint[i], dParamStopCFM, cfm);
    }
}

制御に関しては, 根元の関節の回転を制御するだけなので非常に簡単です.

今回は難しく考えず, 支持脚・遊脚の位相をπだけずらして回転させるようにしました.

動画は以下の通りです.

vimeo.com