氏名 吉田 亮 - lang.sist.chukyo-u.ac.jplang.sist.chukyo-u.ac.jp/classes/seminar/papers/... ·...

2013 年度

卒 業 論 文

シミュレータを用いたロボットアームの制御

指導教員 白井英俊 教授

2014 年 1 月

中京大学 情報理工学部 機械情報工学科

学籍番号 H410126

氏名 吉田 亮

卒業論文要旨

題目 シミュレータを用いたロボットアームの制御

学籍番号 H410126 氏名 吉田 亮 指導教員 白井 英俊

本研究はシミュレーション上で作成したロボットアームの制御を自動化することを目的とした

ものである。シミュレーション用ソフトとして Open Dynamics Engine(以降 ODEと呼ぶ)を使用し

ている。ロボットアームを題材にした動機は、去年先輩が卒業研究でロボットアームの遠隔操作に

苦労していたことにある。私はアームの操作を容易にできないかと考え、そのためにシミュレーシ

ョン上でロボットアームを作成し、制御の自動化の実現を考えた。シミュレータとして ODEを使用

した理由はオープンソースでありながら、商用レベルの品質を備えていることにある。またマニュ

アルが充実しており、車輪型やヒューマノイドなど様々なロボットの制御設計をしながらシミュレ

ーションできるため、ロボットの研究に最適だと考えた。

ODEでは様々な形状の物体を生成し、物体同士をジョイントで結合することができる。物体には

球型、円柱、カプセル型、直方体などの様々な形状があり、ジョイントにはボール、スライダー、

ヒンジの種類がある。本研究では可動域を考慮し、ヒンジジョイントを使用した。

本研究で作成したロボットアームは土台、アーム、ハンドから構成している。土台はカプセル型

であり、将来車型など他のロボットへ拡張しやすくするために設置した。アームは３本の円柱型物

体から生成した。またアームが長すぎるとバランスが取りづらくなることを考慮し適度なサイズに

設定した。ハンドは３本の指(板型の物体)から構成しており、指はそれぞれ２つの部から生成して

いる。直径 15cmの球型物体を下から乗せて囲い込むことを想定し設計をした。

運動の制御として関節角度の目標値と現在地に比例して操作量を変化させる P制御を用いた。コ

マンド関数という関数を作り、各ジョイントのキーボードからのマニュアル操作に成功した。また、

逆運動学を実装することで一回のキーボードでアーム先端を目標値まで近づけることも可能にし

た。

マニュアル操作で対象物をつかんで運ぶことに成功した。そしてそのデータをもとに制御の自動

化実現を目指した。まず土台部にセンサーを取り付け、距離、角度、高さの情報の取得を可能にし

た。検知する物体が球型であると仮定したため、高さの情報はすなわち幅の情報をも表す。そのた

め側面の情報だけで物体をつかむために必要な情報は検知できる。

アーム制御の自動化実現の手段として、シミュレータとアーム制御のプロセスを分離した。お互

いをプロセス間通信によりデータのやりとりを可能にすることで、センサーで取得した物体情報を

もとにアームを動かすことに成功した。また球を任意の位置に出現させることも可能になった。将

来的に機械自身に試行錯誤させ、物体をつかむのに最適なパラメータを求めさせるのも狙いである。

研究結果として、センサーで物体情報を検知し、その情報をもとにアームの先端を物体の近くへ

もっていき、つかむ、運ぶ、放すの一連の動作を行うことに成功した。課題として残ってしまった

部分は球型の形状以外の物体はつかめないことや、機械学習の実装などが挙げられる。

目次

第一章 はじめに ····················································································· 1

第二章 研究の背景 ·················································································· 2 2.1 ODE の概要 ···················································································· 2 2.2 ODE の特徴 ······························· ···················································· 2 2.3 物体の生成 ································ ····················································· 3 2.4 接合ジョイント ·························· ····················································· 3 第三章 ロボットアームの全体設計 ···························································· 3 3.1 全体設計 ························································································· 3 3.4 運動の制御 ······················································································ 5 3.5 キーボードからの操作 ······································································ 5 第四章 手動によるアームの操作 ································································ 6 4.1 アームの関節制御 ············································································· 6

i

4.2 逆運動学 ························································································· 7 4.3 ハンドの関節制御 ············································································· 7 第五章 自動制御 ····················································································· 8 5.1 センサーの実装と運用 ······································································ 8 5.2 アーム制御の自動化·········································································· 9 第六章 結果と考察 ················································································ 10 6.1 結果 ····························································································· 10 6.2 考察 ······························································································ 11 6.3 改善点・展望 ·················································································· 11 参考文献································································································ 12 謝辞 ······································································································ 13 付録 1 ···································································································· 14 付録 2 ···································································································· 37

i

第一章 はじめに

去年先輩が卒業研究でロボットアームの遠隔操作に苦労していた。私はアームの操作を容易

にできないかと考え、シミュレーション上でアーム制御の自動化を実現することを考えた。シ

ミュレーション用ソフトとして Open Dynamics Engine(以降 ODEと呼ぶ) を使用した。ODEを使

用した理由はオープンソースでありながら、商用レベルの品質を備えていることにある。また

マニュアルが充実しており、車輪型やヒューマノイドなど様々なロボットの制御設計をしなが

らシミュレーションできるため、ロボットの研究に最適だと考えた。

まずシミュレータ上でハンドを取り付けたロボットアームを作成する。マニュアル操作を行

えるようにし、そのデータをもとに物体の検出、物体の把持、物体の移動の自動化の実現を目

指した。

結果、ロボットアームのマニュアル操作に成功した。また、センサーで物体情報を検知し、

その情報をもとにアーム先端を物体の近くへもっていき、つかむ、運ぶ、放すの一連の動作を

行うことに成功した。問題点としては、球型の形状以外の物体をつかむことができないことが

挙げられる。

本論文の構成は以下の通りである。

第二章では前提の知識として ODEの概要とその特徴について説明する。第三章ではロボット

アームの全体設計について説明する。また物体の生成、使用した接合ジョイント、運動制御に

ついても説明する。第四章では手動によるアームの操作について説明する。各関節の可動域、

回転軸、逆運動学について説明する。第五章ではセンサーの実装とその運用、アーム制御の自

動化について説明する。第六章では第五章までに述べた研究の結果と課題として残った部分を

説明し、将来への展望について議論する。実際に作成したプログラムは付録として載せている。

1

第二章 研究背景

本研究では Open Dynamics Engine(以降 ODEと呼ぶ)をシミュレータ作成に用いて、ロボット

アームを設計した。そこで、本章では前提知識として ODEの概要とその特徴について説明する。

2.1 ODE の概要

近年、ゲームは本物と見違えるほど精細でリアルな三次元グラフィクスだけではなく、その

仮想空間に登場する人物や車の動き、爆発シーンなどの物理現象がリアルに再現されている。

これらを実現しているのは、物理計算エンジンとよばれるライブラリである(出村 2003, 2007)。

ODE は、ラッセル・スミス氏らが中心となって 2001 年から開発が進められている(Smith,

2007)、オープンソースでありながら商用レベルの品質を備えている物理計算エンジンである。

コンピュータゲーム、3Dの制作ソフトやシミュレータの物理計算エンジンとして広く使われて

おり、多くの大学の講義や研究などにも使われている。

2.2 ODE の特徴

オープンソースである ODEではソースコードを無料で公開しており、誰でも自由にそれを改

良、再配布ができる。そしてマニュアルがしっかりしている。使用言語は C言語。

ODE を使用すれば車輪型やヒューマノイド型など様々なロボットの制御を設計しながらシミ

ュレーションができる。

図 2.1 車輪型ロボット

図 2.2 ヒューマノイド型ロボット

2

2.3 物体の生成

ODE では様々な形状の物体を生成できる。図 2.3 のように左から球型、直方体型、円柱型、

カプセル型がある。それぞれサイズ、位置、姿勢、質量を設定できる。

図 2.3 物体の形状

2.4 接合ジョイント

ODE では物体同士を結合する際、ジョイントを用いる。ジョイントにはスライダー、ボー

ル、ヒンジなどの種類がある。可動域、回転軸、中心点を設定できる。 スライダージョイントではアームの動きに適さず、ボールジョイントは可動域に限度がある

ため、ヒンジジョイントがアームの動きに適していると考えた。本研究では図 2.6 のヒンジジ

ョイントを使用する。

図 2.4 スライダージョイント 図 2.5 ボールジョイント

図 2.6 ヒンジジョイント

3

第三章 ロボットアームの全体設計

本章では本研究で行ったシミュレーション上で作成したロボットアームの全体設計につい

て説明する。なお、DreamCaller (2009)を参考にし、実装には出村(2007)のソースを一部使用

した。

3.1 全体設計

図 3.1 のように将来車型ロボットなど他のロボットに拡張しやすくするため、アームにはカ

プセル型の土台を設置した。サイズは半径 20cm、長さ 10cm とした。 アームは A.B.C３本の円柱型の物体から生成した。アームが長すぎるとバランスが取りづら

くなることを考慮し、A.B のサイズはそれぞれ半径 4cm、長さ 100cm とし、C のサイズは半

径 4cm、長さ 90cm とした。 ハンドは指の黄色部３本と青色部３本から生成している。指を 3 本にした理由は、2 本指の

場合、摩擦係数を設定しているにもかかわらず、物体が滑り落ちてしまう。また、4 本以上の

指だと自由度が高くなり、コントロールが難しくなるためである。図 3.2 のように直径 15cmの球型の物体を下から乗せて囲い込むように持ち上げることを想定し、図 3.1 のように黄色部

のサイズが長さ 20cm、幅 8cm、奥行き 2cm、青色部のサイズが長さ 10cm、幅 8cm、奥行き

2cm の板状とした。

図 3.1 ロボットアームの全体設計

アーム A,アーム B サイズ(半径 4cm, 長さ

100cm) アーム C サイズ(半径 4cm, 長さ

100cm)

土台 サイズ(半径 20cm, 長さ 10cm)

指：黄色部×3 サイズ(長さ 20cm, 幅

8cm, 奥行き 2cm )

指：青色×3 サイズ(長さ 10cm, 幅

8cm, 奥行き 2cm)

4

図 3.2 ハンドがつかんでいる状態

3.4 運動の制御

本研究のシミュレーション上で作成したロボットアームは、関節角度の現在地と目標値の差

に比例して操作量を変化させる P 制御を用いている。 P 制御は目標値から現在値を引き、その差に比例ゲイン(比例定数)を掛けたものを速度とし

て設定している。

3.5 キーボードからの操作

コマンド関数という関数をつくり、キーボード入力した一文字をケース文で判断し、対象と

するジョイントの目標値のパラメータを設定し制御している。

5

第四章 手動によるアームの操作

本章ではキーボード操作のコマンド例、各関節の可動域、回転軸について説明する。

4.1 アームの関節制御

図 4.1～図 4.4 のようにハンドから一番目、二番目の関節は y 軸方向に回転が可能である。

可動域もそれぞれ-135 度～135 度である。写真から見て一番目の関節はキー操作 j で時計回り、

f で反時計回りに回転し、二番目の関節は k で時計回り、d で反時計回りに回転する。

図 4.1 一番目の関節(時計回り) 図 4.2 一番目の関節(反時計回り)

図 4.3 二番目の関節(時計回り) 図 4.4 二番目の関節(反時計回り) 図 4.5～図 4.7 のように土台部の関節は z 軸方向に回転が可能である。可動域は-135 度～135度。キー操作 s で写真から見て時計回り、l で反時計回りに回転する。

図 4.5 時計回り 図 4.6 初期状態 図 4.7 反時計回り

6

4.2 逆運動学

逆運動学も実装した。逆運動学とはアーム先端の目標とする位置を指定し、それを満たすよ

うな各関節角度を求める計算方法である。図 4.8、4.9 のように一回のキーボード入力(G を入

力)でアーム先端を球の近くまでもってくることができる。

図 4.8 初期姿勢 図 4.9 アーム先端を目標値へ近づける

4.3 ハンドの関節制御

図 4.10 のように指の黄色部は一回のキーボード入力(O を入力)で 26 度開く。閉じる際は一

気に閉じると球が弾いてしまうので調整の結果、一回のキーボード入力(C を入力)で 3.6 度閉

じる。 図 4.11 の指の青色部は下から球を乗せることを想定し、一回のキーボード入力(P を入力)で90 度動く。 可動域は黄色部、青色部いずれも 0 度～90 度である。

図 4.10 指:黄色部の動き

逆運動学

7

図 4.11 指:青色部の動き

8

第五章 自動制御

四章までの実装により、アームのマニュアル操作が可能になった。そのデータをもとにアー

ムの制御の自動化をはかる。そのためには物体検知のためにセンサーを取り付け距離、角度、

高さの情報を取得する。そしてその情報をもとにアームの先端を対象物に近づけ、つかむ、運

ぶ、放すの一連の動作自動で行う。 本章ではセンサーの実装と運用、アーム制御の自動化について説明する。

5.1 センサーの実装と運用

シミュレーション上では見えないが、センサーを土台部へ設置した。図 5.1 のようにセンサ

ーには二段階の働きがある。図 5.1 の第一段階では、センサーは xy 平面の方向に-90 度～90度の範囲で物体を探査し、物体のおよその中心位置と距離とを取得する。 図 5.2 の第二段階では、センサーは z 軸のマイナス方向へ物体を探査し、物体の高さの情報

を取得する。ここでは物体が球形をしていると仮定したため、高さの情報はすなわち幅の情報

をも表すことになる。 図 5.1 第一段階 図 5.2 第二段階 図 5.1 センサーによる球形物体の情報取得

ロボット

球

ロボッ

ト 球

9

5.2 アーム制御の自動化 1/2

アーム制御を自動化させる手段として、シミュレータとアーム制御のプロセスを分離させる。

お互いをプロセス間通信によりデータのやりとりすることで、アーム制御プロセスはアームの

具体的な実装にかかわることなく、センサー情報をシミュレータから取得し、また取得した情

報をもとにアームを動かすことができる。また、これはアーム制御と別に、物体を指定した位

置に出現させることも可能にした。これにより、いろいろな位置に物体をおいて、アームの性

能を調べることが容易になった。将来的には、機械自身に試行錯誤させ、球をつかむのに最適

なパラメータを求めさせるのもシミュレータとアーム制御のプロセスを分離した理由である。

図 5.3 シミュレータとアーム制御のプロセスの分離 図 5.2 シミュレータとアーム制御のプロセス分離

5.3 アーム制御の自動化 2/2

図 5.3のように Fを入力すると物体を検知し、アームの制御プロセスに情報が表示される。

図 5.3に示されている thetaはアームからみた物体の方向、distはアームからの距離、 height

は物体の高さ、 widthは幅(アームから水平方向にみた幅であり、誤差を含む)である。

アーム制御のプロセス

シミュレータ

プロセス間

アー

ム制

御命

令

セ ン

サ ー

情 報

デ ー

タ

10

図 5.3 物体情報

この情報がアーム制御プロセスで記憶されているので、次に G を入力すると、指を開き、ア

ーム先端を物体に近い位置に移動させてから指を閉じて物体をつかむことができる(図 5.4)。

図 5.4 物体の把持 物体を持った状態で H を入力すると、アーム先端を茶色の箱(お片づけ箱という位置づけで

ある)の上の位置に移動させ、指を開いて物体を放し、箱のなかに入れることができる(図 5.5)。

図 5.5 物体を箱へ入れる

11

第六章 結果と考察

本章では第五章まで述べた設計により得られた結果と、これからの課題について述べる。

6.1 結果

センサーで取得した情報をもとにアーム先端を物体の近くにもっていき、つかみ、運ぶとい

う一連の動作を行うことに成功した。 表 6.1 に示すように、直径 15cm と 16cm の球型の物体をつかむことに成功した。また表 6.2に示すように、距離が 92cm～114cm の範囲内にある球型物体をつかむことに成功した。 表 6.1 つかめる球の直径 球の直径 つかめたか○or×

14cm ×

15cm ○

16cm ○

17cm ×

18cm ×

表 6.2 つかめる距離の範囲 球の位置 (X, Y)

球との距離

つかめたか ○or×

(68, 68) 89cm × (69, 69) 90cm × (70, 70) 92cm ○ (75, 75) 99cm ○ (80, 80) 106cm ○ (85, 85) 113cm ○ (86, 86) 114cm ○ (87, 87) 116cm ×

12

6.2 考察・改良点

前節で示したように、球型の物体をつかむことに成功しているが、現状では球型以外の形状

の物体をつかむことはできない。 球型の物体の場合、高さの情報を取得すれば幅、半径、奥行きの情報もわかるため、側面の

情報だけで物体を把持するのに必要な情報は得ることができた。しかし、他の形状の物体の場

合は側面の情報だけでは奥行きの情報を得ることができない。解決策としては、もう一つセン

サーを取り付け別の角度からも物体検知を行えるようにすれば必要な情報を取得することが

できる。 また 5.2 のようにシミュレータとアームの制御プロセスを分離させたことで、将来的に機械

自身に試行錯誤をさせ、物体をつかむのに最適なパラメータを求めさせることも考えられる。

手段として、アーム先端に物体をつかんでいるかどうかを判定させるセンサーを取り付ける。

アーム先端の目標値を乱数にし、機械自身に試行錯誤をさせ、物体をつかんだかどうかを判定

させる。物体をつかむのに最適な様々なパラメータを学習させ自律させることが考えられる。

6.3 展望

本研究では実現していないが、同一空間内にもう一台のロボットアームをつくり、物体情報

を共有して一つの物体を協力して持ち上げたり、センサーを取り付けた車型ロボットにアーム

を取り付け移動型ロボットアームにするなど拡張の余地がまだある。

13

参考文献 出村公成 (2003). ODE | demura.net : ロボットの開発と教育.

http://demura.net/ (2013年 10月 31日アクセス)

出村公成 (2007).『簡単！実践！ロボットシミュレーション』. 東京:森北出版.

DreamCaller (2009). ODE-3/22-Robot arm with hand – attached to the body(panel).

http://dreamcaller.tistory.com/entry/ODE-322-Robot-arm-with-hand-attached-to-the-bodypanel

(2013年 6月 25日アクセス)

Smith, Russell (2007). Open Dynamics Engine. http://www.ode.org/ (2013 年 6 月 25

日アクセス)

14

謝辞 本研究を進めるにあたり、ご指導を頂いた卒業論文指導教官の白井英俊教授に感謝致します。

また、日常の議論を通じて多くの知識や示唆を頂いた白井ゼミの同期・後輩の皆さまに感謝い

たします。

15

付録 1 シミュレーター #include <stdio.h> #include <stdlib.h> #include <ode/ode.h> #include <drawstuff/drawstuff.h> #include "texturepath.h" #include <sys/socket.h> #include <netinet/in.h> #include <netdb.h> #include <arpa/inet.h> #include <unistd.h> #include <sys/types.h> #include <unistd.h> #include <fcntl.h> #include <pthread.h> #ifdef _MSC_VER #pragma warning(disable:4244 4305) // for VC++, no precision loss complaints #endif #ifdef dDOUBLE #define dsDrawCapsule dsDrawCapsuleD #define dsDrawCylinder dsDrawCylinderD #define dsDrawSphere dsDrawSphereD #define dsDrawBox dsDrawBoxD #define dsDrawLine dsDrawLineD #endif #define NUM 3 #define FMAX 100 #define Threshold 2.0 #define BallRadius 0.08 // sensor check range #define SearchRange 3.0 #define NumOfClients 1

16

#define BufferSize 128 typedef struct // 構造体を定義 { // char num; char msgBuffer[BufferSize]; } packetArg; packetArg pParams[NumOfClients]; void* processSocket(void* pParam); //スレッド // void* processControl(void* pParam); //スレッド struct sockaddr_in S_IN, client_address[NumOfClients]; int Sockfd; int client_sockfd[NumOfClients];; socklen_t client_len; pthread_mutex_t mutex; pthread_t tid[NumOfClients]; // スレッド識別変数 char commandBuffer[BufferSize]; char msgBuffer[BufferSize]; #define t_Threshold 0.5 #define f_Threshold 0.01 #define Movement 3.0 #define MyPortNo 16561 dWorldID world; // 動力学計算用のワールド dSpaceID space; // 衝突検出用のスペース dGeomID ground; dGeomID ray; dJointGroupID contactgroup; // 接触点グループ dJointID joint[NUM],f_joint, a_joint[NUM], b_joint[NUM],ita_joint,ita2_joint,ita3_joint,ita4_joint; dsFunctions fn; int ANSWER = 1; //bool THETA_f[NUM] = {false,false,false}; typedef struct { dBodyID body; // ボディの ID 番号 dGeomID geom;

17

dReal l,r,m; // ジオメトリの ID 番号 } MyObject ; #define MsgSize 16 #define ArgSize 3 typedef struct // 構造体を定義 { char cmd; int arg[ArgSize]; } CommandLine; float Z = 0.0; dReal dist; MyObject abc, d, e, f, g, h, i, ball, j, k, c, ita, ita2, ita3, ita4; // リンク dReal side[3] = {0.08, 0.02, 0.2}; dReal side_ita[3] = {0.08,0.5,0.5}; dReal side_ita2[3] = {0.5,0.08,0.5}; dReal side_ita3[3] = {0.08,0.5,0.5}; dReal side_ita4[3] = {0.5,0.08,0.5}; dReal f_side[3] = {0.08, 0.02, 0.1}; dReal THETA[NUM] = {0.0, 0.0, 0.0}; dReal f_THETA[NUM] = {0.0, 0.0, 0.0}; dReal b_THETA[NUM] = {0.0, 0.0, 0.0}; static int strLen(char* str) { int i = 0; while (str[i] != '¥0') { i++; } return(i); } static void socketInit() { /* ソケットを生成する */ if ((Sockfd = socket(AF_INET, SOCK_STREAM, 0)) < 0) { perror("server: socket"); exit(1); }

18

/* サーバの情報を与える */ S_IN.sin_family = AF_INET; S_IN.sin_port = htons(MyPortNo); S_IN.sin_addr.s_addr = INADDR_ANY ; bind(Sockfd, (struct sockaddr *)&S_IN, sizeof(S_IN)); listen(Sockfd, 5); for(int i=0; i<NumOfClients; i++) { client_sockfd[i] = accept(Sockfd, (struct sockaddr *)&client_address[i] , &client_len); } // for for(int i=0; i<NumOfClients; i++) { pthread_create(&tid[i], NULL, processSocket, (void*)i); } // for } /* void* sendMessage(void* param) { int num = (int)param; pthread_detach(pthread_self()); printf("=-=-=->>> %s¥n", pParams[num].msgBuffer); write(client_sockfd[num], pParams[num].msgBuffer,strLen(pParams[num].msgBuffer)); return NULL; } */ static CommandLine commandAnalyze(char* line) { CommandLine com; int k, i=0, num=0, len=strLen(line); char msg[MsgSize]; while ((line[i] == ' ') || (line[i] == '¥t')) { i++; } com.cmd = line[i++]; // printf("%s --- %d¥n",line,com.cmd); com.arg[0] = 0; com.arg[1] = 0; for(k=0; k < ArgSize && i<len; k++) { for (num=0; num < MsgSize; msg[num++]='¥0') ; num=0; while ((line[i] == ' ') || (line[i] == '¥t')) { i++; } while ((line[i] != ' ') && (line[i] != '¥t') && (line[i] != '¥n') && (line[i] != '¥0')) {

19

msg[num++] = line[i++]; } msg[num] = '¥0'; com.arg[k] = atoi(msg); printf("DEBUG: com.arg[%d]=%d <-- %s¥n",k,com.arg[k],msg); }return com; } // スレッド void processCommand(CommandLine com, int num); void processMessage(char* str, int num) { // コマンド処理 CommandLine com; printf("==> '%s'¥n",str); // debug // sscanf(str,"%c",&com); com=commandAnalyze(str); // com.cmd, com.arg[2] printf("%c %5d %5d¥n",com.cmd, com.arg[0], com.arg[1]); processCommand(com,num); // sprintf(msgBuffer,"Done¥n"); printf("<-- %s¥n",pParams[num].msgBuffer); // write(client_sockfd[num], pParams[num].msgBuffer,strLen(pParams[num].msgBuffer)); write(client_sockfd[num], pParams[num].msgBuffer,strLen(pParams[num].msgBuffer)); sleep(0.1); } void* processSocket(void* param) { char commandBuffer[BufferSize]; int i, commandFlag = 0; int num=(int)param; pthread_detach(pthread_self()); printf("client [%d]¥n",num); // write(client_sockfd[num], commandBuffer,strLen(commandBuffer)); while(1) { // trial for(i=0; i < BufferSize; commandBuffer[i++] = '¥0') ; commandFlag = read(client_sockfd[num],commandBuffer,sizeof(commandBuffer)); if (commandFlag > 0) {

20

if ((commandBuffer[0] >= ' ') && (commandBuffer[0] <= 'z')) { for(i=0; i < BufferSize; pParams[num].msgBuffer[i++] = '¥0') ; processMessage(commandBuffer,num); } } // if } // while return NULL; } static void nearCallback(void*data, dGeomID o1, dGeomID o2) { dBodyID b1 = dGeomGetBody(o1); dBodyID b2 = dGeomGetBody(o2); if(b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact))return; bool condGroundContact = ((o1 == ground) || (o2 == ground)); static const int N = 10; dContact contact[N]; int n = dCollide(o1, o2, N, &contact[0].geom, sizeof(dContact)); if(condGroundContact) { if(n>0) { for(int i = 0; i < n; i++) { contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactBounce; contact[i].surface.mu = dInfinity; contact[i].surface.soft_erp = 0.5; contact[i].surface.soft_cfm = 1e-4; contact[i].surface.bounce = 0.0; dJointID c =dJointCreateContact(world, contactgroup, &contact[i]); dJointAttach(c, b1, b2); } } } else { if(n>0) {

21

for(int i = 0; i<n; i++) { contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactBounce; contact[i].surface.mu = dInfinity; contact[i].surface.soft_erp = 0.5; contact[i].surface.soft_cfm = 1e-4; contact[i].surface.bounce = 0.0; dJointID c =dJointCreateContact(world, contactgroup, &contact[i]); dJointAttach(c, b1, b2); } } } } void drawBall() { dsSetColor(1.0,0.0,0.0); dsDrawSphere(dBodyGetPosition(ball.body), dBodyGetRotation(ball.body),ball.r); } void drawHand() { dWorldStep(world,0.01); dJointGroupEmpty(contactgroup); dReal r, l; dsSetColor(0.0,1.0,0.0); dGeomCapsuleGetParams(abc.geom, &r, &l); dsDrawCapsule(dBodyGetPosition(abc.body), dBodyGetRotation(abc.body),abc.l,abc.r); dsSetColor(1.0,1.0,0.0); dsDrawBox(dBodyGetPosition(d.body), dBodyGetRotation(d.body),side); dsDrawBox(dBodyGetPosition(e.body), dBodyGetRotation(e.body),side); dsDrawBox(dBodyGetPosition(f.body), dBodyGetRotation(f.body),side);

22

dsSetColor(0.0,0.0,2.0); dsDrawBox(dBodyGetPosition(j.body), dBodyGetRotation(j.body),f_side); dsDrawBox(dBodyGetPosition(k.body), dBodyGetRotation(k.body),f_side); dsDrawBox(dBodyGetPosition(c.body), dBodyGetRotation(c.body),f_side); dsSetColor(0.0,1.0,0.0); dGeomCapsuleGetParams(g.geom, &r, &l); dsDrawCapsule(dBodyGetPosition(g.body), dBodyGetRotation(g.body),g.l,g.r); dGeomCapsuleGetParams(h.geom, &r, &l); dsDrawCapsule(dBodyGetPosition(h.body), dBodyGetRotation(h.body),h.l,h.r); dGeomCapsuleGetParams(i.geom, &r, &l); dsDrawCapsule(dBodyGetPosition(i.body), dBodyGetRotation(i.body),i.l,i.r); dsSetColor(1.0,0.0,0.0); dsDrawSphere(dBodyGetPosition(ball.body), dBodyGetRotation(ball.body),ball.r); dsSetColor(0.5,0.0,0.0); dsDrawBox(dBodyGetPosition(ita.body), dBodyGetRotation(ita.body),side_ita); dsDrawBox(dBodyGetPosition(ita2.body), dBodyGetRotation(ita2.body),side_ita2); dsDrawBox(dBodyGetPosition(ita3.body), dBodyGetRotation(ita3.body),side_ita3); dsDrawBox(dBodyGetPosition(ita4.body), dBodyGetRotation(ita4.body),side_ita4);

23

} void start() { static float xyz[3] = {3.0, 0.0, 1.0}; static float hpr[3] = {-180.0, 0.0, 0.0}; dsSetViewpoint(xyz, hpr); } void makeJoint(int i, MyObject d, dReal x0, dReal y0, dReal z0,dReal x,dReal y,dReal z) { joint[i] = dJointCreateHinge(world, 0); dJointAttach(joint[i], abc.body, d.body); dJointSetHingeAnchor(joint[i], x0, y0, z0); dJointSetHingeParam(joint[i], dParamLoStop, -0.9*M_PI); dJointSetHingeParam(joint[i], dParamHiStop, 0.9*M_PI); dJointSetHingeAxis(joint[i], x, y, z); } void makeBall(dReal x, dReal y, dReal z) { // 1.0, 0.0, 1.0 dMass mass; ball.r = BallRadius; ball.m = 0.01; ball.body = dBodyCreate(world); dMassSetZero(&mass); dMassSetSphereTotal(&mass,ball.m, ball.r); dBodySetMass(ball.body, &mass); dBodySetPosition(ball.body, x,y,z); ball.geom = dCreateSphere(space, ball.r); dGeomSetBody(ball.geom, ball.body); } void makeHand() { dMass mass; abc.r = 0.04; abc.m = 2.00; abc.l = 1.0; d.m = 0.5; e.m = 0.5; f.m = 0.5; j.m = 0.5;

24

k.m = 0.5; c.m = 0.5; g.r = 0.04; g.m = 2.00; g.l = 1.0; h.r = 0.04; h.m = 2.00; h.l = 0.9; i.r = 0.20; i.m = 9.00; i.l = 0.1; ita.m = 0.5; ita2.m = 0.5; ita3.m = 0.5; ita4.m = 0.5; dReal x0 = 0.0, y0 = 0.0, z0 = 0.0; dReal ax = 0, ay = 0, az = 1; dReal x = M_PI/6; dReal y = M_PI*2/3; dReal z = M_PI*4/3; dReal pos[3] = { 1.0, 0.0, 0.0 }; dReal length = 5.0; abc.body = dBodyCreate(world); abc.geom = dCreateCapsule(space, abc.r, abc.l); dGeomSetBody(abc.geom, abc.body); dMassSetZero(&mass); dMassSetCapsuleTotal(&mass, abc.m, 3, abc.r, abc.l); dBodySetMass(abc.body, &mass); dBodySetPosition(abc.body, x0, y0, z0+i.l+h.l+g.l+0.5*abc.l) d.body = dBodyCreate(world); dMassSetZero(&mass); dMassSetBoxTotal(&mass, d.m, side[0], side[1], side[2]); dBodySetMass(d.body,&mass); dBodySetPosition(d.body, x0+abc.r*cos(x), y0+abc.r*sin(x), z0+i.l+h.l+g.l+abc.l+0.5*side[2]); d.geom = dCreateBox(space, side[0], side[1], side[2]); dGeomSetBody(d.geom, d.body); dMatrix3 R; dRFromAxisAndAngle(R, ax, ay, az, 90); dBodySetRotation(d.body,R);

25

e.body = dBodyCreate(world); dMassSetZero(&mass); dMassSetBoxTotal(&mass, e.m, side[0], side[1], side[2]); dBodySetMass(e.body,&mass); dBodySetPosition(e.body, x0-abc.r*cos(x), y0+abc.r*sin(x), z0+i.l+h.l+g.l+abc.l+0.5*side[2]); e.geom = dCreateBox(space, side[0], side[1], side[2]); dGeomSetBody(e.geom, e.body); dRFromAxisAndAngle(R, ax, ay, az, -90); dBodySetRotation(e.body,R); f.body = dBodyCreate(world); dMassSetZero(&mass); dMassSetBoxTotal(&mass, f.m, side[0], side[1], side[2]); dBodySetMass(f.body,&mass); dBodySetPosition(f.body, x0, y0-abc.r, z0+i.l+h.l+g.l+abc.l+0.5*side[2]); f.geom = dCreateBox(space, side[0], side[1], side[2]); dGeomSetBody(f.geom, f.body); j.body = dBodyCreate(world); dMassSetZero(&mass); dMassSetBoxTotal(&mass, j.m, f_side[0], f_side[1], f_side[2]); dBodySetMass(j.body,&mass); dBodySetPosition(j.body, x0, y0-abc.r, z0+i.l+h.l+g.l+abc.l+side[2]+0.5*f_side[2]); j.geom = dCreateBox(space, f_side[0], f_side[1], f_side[2]); dGeomSetBody(j.geom, j.body); k.body = dBodyCreate(world); dMassSetZero(&mass); dMassSetBoxTotal(&mass, k.m, f_side[0], f_side[1], f_side[2]); dBodySetMass(k.body,&mass); dBodySetPosition(k.body, x0-abc.r*cos(x), y0+abc.r*sin(x), z0+i.l+h.l+g.l+abc.l+side[2]+0.5*f_side[2]); k.geom = dCreateBox(space, f_side[0], f_side[1], f_side[2]); dGeomSetBody(k.geom, k.body); dRFromAxisAndAngle(R, ax, ay, az, -90); dBodySetRotation(k.body, R);

26

c.body = dBodyCreate(world); dMassSetZero(&mass); dMassSetBoxTotal(&mass, c.m, f_side[0], f_side[1], f_side[2]); dBodySetMass(c.body,&mass); dBodySetPosition(c.body, x0+abc.r*cos(x), y0+abc.r*sin(x), z0+i.l+h.l+g.l+abc.l+side[2]+0.5*f_side[2]); c.geom = dCreateBox(space, f_side[0], f_side[1], f_side[2]); dGeomSetBody(c.geom, c.body); dRFromAxisAndAngle(R, ax, ay, az, 90); dBodySetRotation(c.body, R); g.body = dBodyCreate(world); g.geom = dCreateCapsule(space, g.r, g.l); dGeomSetBody(g.geom, g.body); dMassSetZero(&mass); dMassSetCapsuleTotal(&mass, g.m, 3, g.r, g.l); dBodySetMass(g.body, &mass); dBodySetPosition(g.body, x0, y0, z0+i.l+h.l+0.5*g.l); h.body = dBodyCreate(world); h.geom = dCreateCapsule(space, h.r, h.l); dGeomSetBody(h.geom, h.body); dMassSetZero(&mass); dMassSetCapsuleTotal(&mass, h.m, 3, h.r, h.l); dBodySetMass(h.body, &mass); dBodySetPosition(h.body, x0, y0, z0+i.l+0.5*h.l); // dRFromAxisAndAngle(R, 1, 0, 0, M_PI/2); // dBodySetRotation(h.body, R); i.body = dBodyCreate(world); i.geom = dCreateCapsule(space, i.r, i.l); dGeomSetBody(i.geom, i.body); dMassSetZero(&mass); dMassSetCapsuleTotal(&mass, i.m, 3, i.r, i.l); dBodySetMass(i.body, &mass); dBodySetPosition(i.body, x0, y0, z0+0.5*i.l);

27

ita.body = dBodyCreate(world); dMassSetZero(&mass); dMassSetBoxTotal(&mass, ita.m, side_ita[0], side_ita[1], side_ita[2]); dBodySetMass(ita.body,&mass); dBodySetPosition(ita.body, -0.5, 1.0, 0.0); ita.geom = dCreateBox(space, side_ita[0], side_ita[1], side_ita[2]); dGeomSetBody(ita.geom, ita.body); ita2.body = dBodyCreate(world); dMassSetZero(&mass); dMassSetBoxTotal(&mass, ita2.m, side_ita2[0], side_ita2[1], side_ita2[2]); dBodySetMass(ita2.body,&mass); dBodySetPosition(ita2.body, -0.3, 0.7, 0.0); ita2.geom = dCreateBox(space, side_ita2[0], side_ita2[1], side_ita2[2]); dGeomSetBody(ita2.geom, ita2.body); ita3.body = dBodyCreate(world); dMassSetZero(&mass); dMassSetBoxTotal(&mass, ita3.m, side_ita3[0], side_ita3[1], side_ita3[2]); dBodySetMass(ita3.body,&mass); dBodySetPosition(ita3.body, -0.1, 1.0, 0.0); ita3.geom = dCreateBox(space, side_ita3[0], side_ita3[1], side_ita3[2]); dGeomSetBody(ita3.geom, ita3.body); ita4.body = dBodyCreate(world); dMassSetZero(&mass); dMassSetBoxTotal(&mass, ita4.m, side_ita4[0], side_ita4[1], side_ita4[2]); dBodySetMass(ita4.body,&mass); dBodySetPosition(ita4.body, -0.3, 1.3, 0.0); ita4.geom = dCreateBox(space, side_ita4[0], side_ita4[1], side_ita4[2]); dGeomSetBody(ita4.geom, ita4.body); ita_joint = dJointCreateFixed(world, 0); dJointAttach(ita_joint, ita.body, 0); dJointSetFixed(ita_joint); ita2_joint = dJointCreateFixed(world, 0); dJointAttach(ita2_joint, ita2.body, 0); dJointSetFixed(ita2_joint);

28

ita3_joint = dJointCreateFixed(world, 0); dJointAttach(ita3_joint, ita3.body, 0); dJointSetFixed(ita3_joint); ita4_joint = dJointCreateFixed(world, 0); dJointAttach(ita4_joint, ita4.body, 0); dJointSetFixed(ita4_joint); f_joint = dJointCreateFixed(world, 0); dJointAttach(f_joint, i.body, 0); dJointSetFixed(f_joint); a_joint[0] = dJointCreateHinge(world, 0); dJointAttach(a_joint[0], abc.body, g.body); dJointSetHingeAnchor(a_joint[0], x0, y0, z0+i.l+h.l+g.l); dJointSetHingeAxis(a_joint[0], 0, 1, 0); dJointSetHingeParam(a_joint[0], dParamLoStop, -0.75*M_PI); dJointSetHingeParam(a_joint[0], dParamHiStop, 0.75*M_PI); a_joint[1] = dJointCreateHinge(world, 0); dJointAttach(a_joint[1], g.body, h.body); dJointSetHingeAnchor(a_joint[1], x0, y0, z0+i.l+h.l); dJointSetHingeAxis(a_joint[1], 0, 1, 0); dJointSetHingeParam(a_joint[1], dParamLoStop, -0.75*M_PI); dJointSetHingeParam(a_joint[1], dParamHiStop, 0.75*M_PI); a_joint[2] = dJointCreateHinge(world, 0); dJointAttach(a_joint[2], h.body, i.body); dJointSetHingeAnchor(a_joint[2], x0, y0, z0+i.l); dJointSetHingeAxis(a_joint[2], 0, 0, 1); dJointSetHingeParam(a_joint[2], dParamLoStop, -0.75*M_PI); dJointSetHingeParam(a_joint[2], dParamHiStop, 0.75*M_PI); b_joint[0] = dJointCreateHinge(world, 0); dJointAttach(b_joint[0], j.body, f.body); dJointSetHingeAnchor(b_joint[0], x0, y0-abc.r, z0+i.l+h.l+g.l+abc.l+side[2]); dJointSetHingeAxis(b_joint[0], -1, 0, 0); dJointSetHingeParam(b_joint[0], dParamLoStop, -0.5*M_PI); dJointSetHingeParam(b_joint[0], dParamHiStop, 0.5*M_PI);

29

b_joint[1] = dJointCreateHinge(world, 0); dJointAttach(b_joint[1], e.body, k.body); dJointSetHingeAnchor(b_joint[1], x0-abc.r*cos(x), y0+abc.r*sin(x), z0+i.l+h.l+g.l+abc.l+side[2]); dJointSetHingeAxis(b_joint[1], 1*cos(z), 1*sin(z), 0); dJointSetHingeParam(b_joint[1], dParamLoStop, -0.5*M_PI); dJointSetHingeParam(b_joint[1], dParamHiStop, 0.5*M_PI); b_joint[2] = dJointCreateHinge(world, 0); dJointAttach(b_joint[2], d.body, c.body); dJointSetHingeAnchor(b_joint[2], x0+abc.r*cos(x), y0+abc.r*sin(x), z0+i.l+h.l+g.l+abc.l+side[2]); dJointSetHingeAxis(b_joint[2], 1*cos(y), 1*sin(y), 0); dJointSetHingeParam(b_joint[2], dParamLoStop, -0.5*M_PI); dJointSetHingeParam(b_joint[2], dParamHiStop, 0.5*M_PI); makeJoint(0, d, x0+abc.r*cos(x), y0+abc.r*sin(x), z0+i.l+h.l+g.l+abc.l, 1*cos(y), 1*sin(y), 0); makeJoint(1, e, x0-abc.r*cos(x), y0+abc.r*sin(x), z0+i.l+h.l+g.l+abc.l, 1*cos(z) ,1*sin(z), 0); makeJoint(2, f, x0, y0-abc.r, z0+i.l+h.l+g.l+abc.l, 1, 0, 0); } dReal checkSensor(int theta, float height){ int i; // dsSetColor( 1.0, 0.0, 0.0 ); dReal length = SearchRange; dReal p[3], endPoint[3], minDist = SearchRange; dContactGeom contact; dGeomID ray = dCreateRay(0, length); // space は 0 でないといけない p[0] = p[1] = 0.0; p[2] = height; endPoint[0]=(length*cos(M_PI*theta/180.0)); endPoint[1]=(length*sin(M_PI*theta/180.0)); endPoint[2]=0.0; dGeomRaySet(ray, p[0],p[1],p[2],endPoint[0],endPoint[1],endPoint[2]); for(i=0; i<3; i++, endPoint[i] += p[i]); // printf("End point: %10.3f, %10.3f, %10.3f¥n", endPoint[0], endPoint[1], endPoint[2]); // dsDrawLine(p,endPoint); sleep(1.0); // 瞬間的に描画されている if (dCollide(ball.geom,ray,1,&contact,sizeof(dContact))==1 ) {

30

if (minDist > contact.depth) minDist=contact.depth; } return(minDist); // 障害物の表面までの距離を返す(最大 SearchRange) } void inverseKinematics(dReal Px, dReal Py, dReal Pz){ dReal l[4] = {0.10, 0.90, 1.00, 1.00}; dReal tmpL = sqrt(Px*Px+Py*Py); dReal P1P = sqrt(Px*Px+Py*Py+(Pz-(l[0]+l[1])) * (Pz-(l[0]+l[1]))); dReal Ca = (l[2]*l[2]+P1P*P1P-l[3]*l[3])/(2*l[2]*P1P); dReal phi = atan2(Pz-(l[0]+l[1]), tmpL); dReal alpha = atan2(sqrt(1 - Ca * Ca), Ca); dReal Cb = (l[2]*l[2]+l[3]*l[3]-P1P*P1P)/(2*l[2]*l[3]); dReal beta = atan2(sqrt(1- Cb*Cb), Cb); dReal beta2 = acos(Px/1.0); //new dReal beta3 = M_PI - ((M_PI/2) + beta2); switch(ANSWER){ case 1: THETA[2] = atan2(Py, Px); THETA[1] = M_PI/2 - phi - alpha; //M_PI/2 - beta2; // new THETA[0] = M_PI - beta; break; case 2: THETA[2] = atan2(Py, Px); THETA[1] = M_PI/2 - phi + alpha; THETA[0] = M_PI + beta; break; case 3: THETA[2] = atan2(Py, Px) + M_PI; THETA[1] = -(M_PI/2 - phi - alpha); THETA[0] = M_PI + beta; break; case 4: THETA[2] = atan2(Py, Px) + M_PI; THETA[1] = -(M_PI/2 - phi + alpha); THETA[0] = M_PI - beta; break; } } void Pcontrol()

31

{ dReal k = 2.0 ; // Original: 2.0 dReal k_f = 0.5; // Original: 0.5 dReal fMax = 300.0; for (int u = 0; u < NUM; u++){ dReal tmp = dJointGetHingeAngle(a_joint[u]); dReal z = THETA[u] - tmp; dJointSetHingeParam(a_joint[u], dParamVel, k*z ); dJointSetHingeParam(a_joint[u], dParamFMax, fMax); } for (int v = 0; v < NUM; v++){ dReal tmp = dJointGetHingeAngle(joint[v]); dReal z = f_THETA[v] - tmp; dJointSetHingeParam(joint[v], dParamVel, k*z); dJointSetHingeParam(joint[v], dParamFMax, fMax); } for (int t = 0; t < NUM; t++){ //THETA_f[t] == true; dReal tmp = dJointGetHingeAngle(b_joint[t]); dReal z = b_THETA[t] - tmp; dJointSetHingeParam(b_joint[t], dParamVel, k_f*z); dJointSetHingeParam(b_joint[t], dParamFMax, 300); } } void keyCommand(int cmd) { int i; switch (cmd) { case 'j': THETA[0] += M_PI/180; break; case 'f': THETA[0] -= M_PI/180; break; case 'k': THETA[1] += M_PI/180; break; case 'd': THETA[1] -= M_PI/180; break;

32

case 'l': THETA[2] += M_PI/180; break; case 's': THETA[2] -= M_PI/180; break; case 'p': f_THETA[0] += M_PI/180; f_THETA[1] += M_PI/180; f_THETA[2] += M_PI/180; break; case 'q': f_THETA[0] -= M_PI/180; f_THETA[1] -= M_PI/180; f_THETA[2] -= M_PI/180; break; case 'o': b_THETA[0] += M_PI/180; b_THETA[1] += M_PI/180; b_THETA[2] += M_PI/180;break; case 'w': b_THETA[0] -= M_PI/180; b_THETA[1] -= M_PI/180; b_THETA[2] -= M_PI/180; break; case 'r': dJointGroupDestroy(contactgroup); dBodyDestroy(ball.body); // dBodyDestroy(ob.body); dBodyDestroy(abc.body); contactgroup = dJointGroupCreate(0); for(i=0; i<NUM; i++){ THETA[i] = b_THETA[i] = f_THETA[i] = 0; } makeHand(); break; case 'A': for(i=-30; i<= 30; i+=3) { printf("theta = %d [deg], distance = %10.3f¥n",i, checkSensor(i,BallRadius)); } break; case 'h': inverseKinematics(-0.25, 0.95, 0.8); break; case 'G': inverseKinematics(1.0, 0.5, 1.2); break; case 'g': inverseKinematics(1.0, 0.0, 0.8);break; /* case '+': Z = Z+0.05; printf("Z = %10.3f¥n",Z); break; case '-': Z = Z-0.05; printf("Z = %10.3f¥n",Z); break; case 'Z': inverseKinematics(1.0, 0.0, 0.5+Z); break; */ case '!':

33

dSpaceDestroy(space); dWorldDestroy(world); dCloseODE(); exit(0); } } void processCommand(CommandLine com,int num) { switch (com.cmd){ case 'L': // sensor Check sprintf(pParams[num].msgBuffer,"S %d¥n",(int)(checkSensor(com.arg[0],com.arg[1]/100.0)*100)); break; case 'M': // move the hand inverseKinematics(com.arg[0]/100.0, com.arg[1]/100.0, com.arg[2]/100.0); sprintf(pParams[num].msgBuffer,"called inverseKinematics(%10.3f,%10.3f,%10.3f)¥n",com.arg[0]/100.0, com.arg[1]/100.0, com.arg[2]/100.0); break; case 'N': dBodyDestroy(ball.body); dGeomDestroy(ball.geom); makeBall(com.arg[0]/100.0, com.arg[1]/100.0, BallRadius*1.2); sprintf(pParams[num].msgBuffer,"New ball...¥n"); break; case 'O': f_THETA[0] -= M_PI/7; f_THETA[1] -= M_PI/7; f_THETA[2] -= M_PI/7; sprintf(pParams[num].msgBuffer,"palm Open¥n"); break; case 'C': f_THETA[0] += M_PI/50; f_THETA[1] += M_PI/50; f_THETA[2] += M_PI/50; sprintf(pParams[num].msgBuffer,"palm Closing¥n"); break; case 'P': b_THETA[0] += M_PI/1.5; b_THETA[1] += M_PI/1.5; b_THETA[2] += M_PI/1.5; sprintf(pParams[num].msgBuffer,"To Pick Something¥n"); break; case 'D': b_THETA[0] -= M_PI/1.5; b_THETA[1] -= M_PI/1.5;

34

b_THETA[2] -= M_PI/1.5; sprintf(pParams[num].msgBuffer,"To Drop Something¥n"); break; case 'T': ANSWER = com.arg[0]; sprintf(pParams[num].msgBuffer,"Poise Mode = %d¥n",ANSWER); break; case 'Y': for(int i=0; i<3; i++) { f_THETA[i]=0.0; b_THETA[i]=0.0; } sprintf(pParams[num].msgBuffer,"Palm closing...¥n"); break; case 'R': // reset the position for(int i=0; i<3; i++) { THETA[i]= 0.0; f_THETA[i]=0.0; b_THETA[i]=0.0; } sprintf(pParams[num].msgBuffer,"Back to the normal position¥n"); break; default: keyCommand(com.cmd); sprintf(pParams[num].msgBuffer,"Done"); } } static void simLoop(int pause) { /* char commandBuffer[BufferSize]; char com, msgBuffer[] = "Done¥n"; int commandFlag = 0; commandBuffer[0] = '¥0'; commandFlag = read(client_sockfd,commandBuffer,sizeof(commandBuffer)); if (commandFlag > 0) { printf("== '%s'¥n",commandBuffer); // debug sscanf(str,"%c",&com); // 一文字コマンドを仮定 processCommand(com); // printf("->'%s'¥n",msgBuffer); // write(client_sockfd, msgBuffer,strLen(msgBuffer)); write(client_sockfd, msgBuffer,(size_t)msgBuffer);

35

sleep(0.1); } // if */ Pcontrol(); //control(); dSpaceCollide(space, 0, &nearCallback); dWorldStep(world, 0.01); dJointGroupEmpty(contactgroup); drawHand(); drawBall(); /* if (dist>=0.0) { printf( "dist = %f¥n", dist ); } dsSetColor( 1.0f, 0.0f, 0.0f ); dVector3 Origin, Direction; dGeomRayGet(geom_ray, Origin, Direction); dReal Length = dGeomRayGetLength(geom_ray); dVector3 endPoint; endPoint[0] = Origin[0] + (Direction[0] * Length); endPoint[1] = Origin[1] + (Direction[1] * Length); endPoint[2] = Origin[2] + (Direction[2] * Length); // endPoint[3] = Origin[3] + (Direction[3] * Length); dsDrawLine(Origin, End); */ /* static const int N = 10; dContact contact[N]; if(THETA_f[0] == true){ if(dCollide(j.geom, ball.geom, N, &contact[N].geom, sizeof(dContact)) > 0){ dJointSetHingeParam(b_joint[0], dParamVel, 0.0); THETA_f[0] = false; } }

36

if(THETA_f[1] == true){ if(dCollide(k.geom, ball.geom, N, &contact[N].geom, sizeof(dContact)) > 0){ dJointSetHingeParam(b_joint[1], dParamVel, 0.0); THETA_f[1] = false; } } if(THETA_f[2] == true){ if(dCollide(c.geom, ball.geom, N, &contact[N].geom, sizeof(dContact)) > 0){ dJointSetHingeParam(b_joint[2], dParamVel, 0.0); THETA_f[2] = false; } } */ } void setDrawStuff() { fn.version = DS_VERSION; fn.start = &start; fn.step = &simLoop; fn.command = &keyCommand; fn.path_to_textures = "../../drawstuff/textures"; } int main(int argc, char *argv[]) { dInitODE(); //ＯＤＥの初期化 setDrawStuff(); world = dWorldCreate(); // ワールドの生成 space = dHashSpaceCreate(0); // スペースの生成 contactgroup = dJointGroupCreate(0); // 接触グループの生成 ground = dCreatePlane(space, 0, 0, 1, 0); // 地面の生成 dWorldSetGravity(world, 0, 0, -9.8); makeHand();

37

makeBall(1.0, 0.0, 1.0); socketInit(); dsSimulationLoop(argc, argv, 640, 480, &fn); // シミュレーションループ dSpaceDestroy(space); // スペースの破壊 dWorldDestroy(world); // ワールドの破壊 dCloseODE(); //ＯＤＥの終了 return 0; }

38

付録 2 アーム制御のプロセス

#include <stdlib.h> #include <sys/types.h> #include <sys/socket.h> #include <stdio.h> #include <netinet/in.h> #include <arpa/inet.h> #include <unistd.h> #include <math.h> #include <sys/stat.h> #include <fcntl.h> #include <sys/select.h> #include <sys/time.h> #define BufferSize 128 #define MsgSize 16 #define ArgSize 3 #define SearchAngle 90 #define SearchStep 3 #define SearchRange 300 #define Adjustment 2 #define BallRadius 8 #define HeightMax 20 #define HeightMin 4 #define HeightStep 4 #define RegulatedHeight 28 #define BoxX -25 #define BoxY 95 #define BoxHeight 50 #define PI 3.1415926536 #define PI_DEG 180.0

39

//ver0.5: timeout を追加 typedef struct // 探索した物体への角度、高さ、距離 { int theta, dist, height; float width; // 幅 } SearchedObject; SearchedObject target; int SockFd ; struct sockaddr_in Address ; char Buf[BufferSize], Buf2[BufferSize]; void waitingResponse(char* buf) { struct timeval timeout; timeout.tv_sec = 0; timeout.tv_usec = 500000; fd_set rfds; int accessible; FD_ZERO(&rfds); FD_SET(SockFd, &rfds); // int select(int 走査ディスクリプタ数, fd_set *読込ディスクリプタ集合, // fd_set *書込ディスクリプタ集合, fd_set *例外ディスクリプタ集合, //timeval *時間切れ時間); write(SockFd,buf,BufferSize); sleep(0.5); while ((accessible = select(FD_SETSIZE, &rfds, NULL, NULL, &timeout)) == 0) { write(SockFd,buf,BufferSize); sleep(((float)rand())/RAND_MAX); // printf("%s 's response waiting %f sec¥n",buf, (float)(rand())/RAND_MAX); timeout.tv_sec = 0; timeout.tv_usec = 500000; } } int rangeSensor(int theta, int height) { char buf[BufferSize], buf2[BufferSize];

40

int i, len; for(len=0; len<BufferSize; buf[len]='¥0',buf2[len]='¥0',len++) ; sprintf(buf,"L %d %d¥n", theta, height); // printf("%s (%d) called¥n", buf, sizeof(buf)); waitingResponse(buf); read(SockFd,buf2,BufferSize); sscanf(buf2, "S %d",&len); // printf("%s (%d) Returns: %s => %d¥n", buf, sizeof(buf), buf2, len); return(len); } int findObjectSub(SearchedObject o) { int h, temp=target.dist, dist=target.dist, step = -1; printf("Finding...: theta=%d, dist=%d, height=%d, width =%d¥n", target.theta, target.dist, target.height, target.width); for (h=HeightMax; h > 0; h--) { if (rangeSensor(target.theta,h) < SearchRange) // find something { target.height = h; break; } sleep(0.1); } if (target.theta < 0) { step = 1; } for (h=target.theta; temp < SearchRange ; h+=step) { temp = rangeSensor(h,target.height); sleep(0.1); if (temp >= SearchRange) { printf("dist = %d, theta = %d, sin(theta)=%f¥n",dist,(h-target.theta),sin(abs(h-target.theta)*PI/PI_DEG)); target.width = dist*sin(abs(h-target.theta)*PI/PI_DEG)*2.0; break; } else { dist = temp; } } return(target.theta); } int findObject(int theta) { int len, i, sMax = theta+SearchAngle;

41

target.dist = SearchRange; for(i=theta-SearchAngle; i<=sMax; i+= SearchStep) { len = rangeSensor(i,HeightMin); if (len < target.dist) { target.dist = len; target.theta = i; target.height = HeightMin; printf("Sensor Data --- %d, %d => %d¥n", target.theta, target.height, target.dist); } else { if ((target.dist < SearchRange) && (len > target.dist)) break; } } // for if (target.dist <= SearchRange) return(findObjectSub(target)); else return(SearchRange); // 300 is bogus } void socketInit(void) { int len, result; SockFd = socket(AF_INET,SOCK_STREAM,0); Address.sin_family = AF_INET ; Address.sin_addr.s_addr = inet_addr("127.0.0.1"); Address.sin_port = htons(16561) ; len = sizeof(Address); result = connect(SockFd , (struct sockaddr *)&Address , len); if ( result == -1 ) { perror("oops: Server doesn't respond"); exit(1); } } void moveArm(int x,int y,int z) { int i; char buf[BufferSize], buf2[BufferSize];

42

printf("Move to %d, %d, %d¥n",x,y,z); for(i=0;i < BufferSize; buf[i] ='¥0', buf2[i]='¥0', i++); sprintf(buf, "M %d %d %d¥n",x,y,z); write(SockFd,buf,BufferSize); sleep(0.1); while (read(SockFd,buf2,BufferSize) == 0) { sleep(0.1); }; printf("%s¥n",buf2); } void talk(char cmd) { int i; char buf[BufferSize], buf2[BufferSize]; for(i=0;i < BufferSize; buf[i] ='¥0', buf2[i]='¥0', i++); sprintf(buf, "%c¥n",cmd); waitingResponse(buf); read(SockFd,buf2,BufferSize); printf("%s¥n",buf2); } void communicate(char* cmd) { int i; char buf[BufferSize], buf2[BufferSize]; for(i=0;i < BufferSize; buf[i] ='¥0', buf2[i]='¥0', i++); sprintf(buf, "%s¥n",cmd); waitingResponse(buf); read(SockFd,buf2,BufferSize); printf("%s¥n",buf2); } int main(void) { int theta, c; socketInit();

43

printf("---- Robot Arm & Finger Controller ver. 0.5 ---(RAND_MAX=%d)¥n",RAND_MAX); while (true) { printf("> "); gets(Buf); // printf("'%s'¥n",buf); switch(Buf[0]) { case '!': talk('!'); printf("Controller halts¥n"); exit(0); case 'F': // printf("Find Object: %d¥n", findObject()); sscanf(Buf,"%c %d",&c, &theta); if ((theta > PI_DEG) || (theta < -PI_DEG)) theta=0; printf("Search from %d¥n",theta); findObject(theta); printf("Find Object: theta=%d, dist=%d, height=%d, width =%d¥n", target.theta, target.dist, target.height, (int)target.width); break; case 'G': if (target.dist < SearchRange) { talk('1'); sleep(1); moveArm((int)target.dist*cos(target.theta*PI/PI_DEG)+BallRadius-Adjustment, (int)target.dist*sin(target.theta*PI/PI_DEG), RegulatedHeight*4); sleep(1); talk('O'); sleep(1); // Open moveArm((int)target.dist*cos(target.theta*PI/PI_DEG)+BallRadius-Adjustment, (int)target.dist*sin(target.theta*PI/PI_DEG), RegulatedHeight+Adjustment); sleep(1); talk('P'); sleep(1); talk('C'); sleep(1); moveArm((int)target.dist*cos(target.theta*PI/PI_DEG)+BallRadius-Adjustment, (int)target.dist*sin(target.theta*PI/PI_DEG), target.height*3); } else { printf("Find the object first!¥n"); } break; case 'H': // talk('h'); moveArm(BoxX, BoxY, BoxHeight+target.height*2); // "above the box"

44

sleep(1); moveArm(BoxX, BoxY, RegulatedHeight+target.height); // "above the box" sleep(1); talk('O'); break; default: communicate(Buf); sleep(0.1); } // switch } // while close(SockFd); exit(0); }

45

46

47

48