CoppeliaSimによる動作教示

ここではロボットシミュレータCoppeliaSimを使って、ロボットに動作教示をする方法を示す。

●動作教示のための準備（CoppeliaSimファイルの作成）

• Autodesk Inventorなどの３D CADでSTLファイルを作る。このときロボットがまっすぐ立っているように軸を合わせる（Z軸を重力方向上向きに取る）。また腕やしっぽなどはｘ，ｙ，ｚのどれかの軸に平行に作っておく方が良い。また各関節位置の座標値をメモしておくことが望ましい。

• 作成したSTLファイルをCoppeliaSimで読み込む。

• 読み込んだモデルをScene hierarchyのWindowで選択し、右クリック→Edit→Grouping/Merging→Divide selected shapesでパーツ毎に分割する。

• 分割したパーツをパーツ間で動きがない部分（固まって可動する部分）毎に新たなパーツとして設定する。

• 分割したパーツをベース（地面に設置している部分）の上に順に階層的にScene hierarchy内で組み上げていく。子となるパーツを親となるパーツの上にドラッグ＆ドロップすれば良い。

• 組み上げたパーツ間にjointを設定する。Scene上で右クリック→Add→Joint→Revoluteで回転ジョイントを設定する。

• 作成したジョイントはScene hierarchyで選択→ダブルクリックし、出てくるダイアログで joint is in inverse kinematics mode とする。

●逆運動学（Inverse kinematics）の設定の仕方

• ロボットの手先位置などを決めて，そこに動かすためには逆運動学（手先の3次元座標値を達成するためのロボットの関節角計算）を解く必要があ る。今回の実習ではこれを直接解かずにCoppeliaSimというシミュレータに内蔵されたInverse kinematicsを使って関節角を教示する。

• CoppeliaSim での設定は手先位置にDummyと呼ばれるオブジェクトを設定し，適当な名前（例えば右アームの手先ならrTipなど）に変更しておきます。Scene上で右クリック→Add→Dummyで設定し，その位置を手先などに合わせる。合わせ方は手先に手先部分だけのパーツがある場合には先のDummy（rTip）をScene hierarchyで選択した後，手先のパーツをShiftを押しながら選択する。そしてObject/Item position/orientationダイアログを出し，Object/item positionのところのApply to selectionボタンを押すと，Dummyの位置が手先のパーツの位置と同じになる。手先に適当なパーツがない場合にはｘ，ｙ，ｚの数値で合わせたり，近いパーツを選んでそこに移動させてから数値で合わせることになる。

• 手先などのDummyが設定できたら，そのDummyをコピーし，同じものをもう１つ作る（ペーストする）。出来たDummyをターゲットとする（このターゲットを動かして，それに対して最初のDummyが追従することで手先などの目標を操作する。

• 手先（aTip）とそのターゲット（aTarget）が作成できたら，どちから一方をScene hierarchyで選択し，ダブルクリックする。すると下記のダイアログが出てくる。

• 上のダイアログの中程のDummy – dummy linkingでaTipに対してaTargetを選択する。するとLink typeがIK, tip-targetとなる。ここでリンクされた2つのDummyは下の図のようにScene hierarchyで赤い矢印で結ばれる。各TipのDummyはTargetによって誘導されるパーツ上 に置く。下の図ではBodyの Tip（Btip）はBody(BD)に，右腕，左腕のRtip, Ltipはそれぞれ手先のパーツRAM, LAM上に置いている。図の手先に付いている小さな球がTipのDummyである。また誘導に使うターゲットは誘導されるIK group（9.参照）が乗っているパーツに乗せるようにする（図参照）。

• 次にIK（Inverse kinematics）の設定を行う。上の図のScene hierarchyの左にあるツールパレットの上から3つ目のｆ（ｘ）をクリックする（またはメニューのTools→Calculation module propertiesを選択する）。すると次のダイアログが現れる。このダイアログの上2行のボタン群の2行目の一番左のInverse kinematicsボタンを押した状態が下である。ここでIK groupとはIKを考える単位となるグループで，IK groupの根本から手先までのIKが計算される。このIK group毎に先ほどDummy-dummy linkingで指定したペアを設定する。下のダイアログのAdd new IK groupボタンを押すと，その下のフィールドにIK_Groupが現れる。必要ならここをクリックして適当な名前に変更する。下のダイアログでは Rarm, Larm, bodyに変更している。またそれぞれのIK group毎に下記のIK group propertiesを設定する。ここでは全てのgroupに対し，Calc. method=DLS，Damping=0.3としている。

• Rarmを例にとってIKの設定方法を説明する。まず下のダイアログの右上のドロップダウンメニューのRtipを選択し，Addo new IK element with tip:ボタンを押す。

• Rtipが選択された状態で下記のようになる。

• Element is activeがチェックされていることを確認し，Baseを右腕が乗っているBDに設定する。

• 動かす自由度の設定をConstraintsのところで行う。ここではｘ，ｙ，ｚの位置のみ動かすように設定している。姿勢も動かしたい場合にはAlpha-beta, Gammaをチェックする。

• 以上をすべてのIK groupに対して行う。

●動作教示（辻盆などの動作をシミュレータのロボットを動かして教示。教示点は動きの節目ごとの点を与えること。教示点間は関節毎に関節角を等分して20ms毎のデータとして，次の20msデータ作成のステップで補間される。）

• CoppeliaSim を使い，上で作ったtttファイル（CoppeliaSim 用ファイル）のモデルの大元（BaseあるいはBody。そこから枝分かれしている大元）に右クリッ ク→Add→Associated child script→Non threadedを選択し，child scriptを作成する。Child scriptのプログラム言語はLua。

• Child scriptを開け（モデルの横のノートのマークをダブルクリック），example.ttt（拡張子をtttに変更すること）のUnder_BodyにあるChild script（Under_Bodyの右にあるノートのマークをダブルクリックすると出てくる）をコピペする。ペーストするときは最初に出てくるプログラ ムは全部消してからペーストする。

• 下記のChild Script（1.で作ったtttファイル内のスクリプト）に示す赤字部分を自分の作ったモデルのジョイント名に変更する。また青字部分を自分でわかり易い名前を考えて変更する。

• シミュレータを実行し，targetを動かして辻盆の必要なポーズを教示し，データを生成する。

  • 動 かない場合にはCoppeliaSim メニューのTools→Calculation module propertiesでInverse kinematicsをクリックし，下のIK groupsのところにある2つのIK groupをそれぞれ選択後，一番下のEdit IK elementsボタンを押す。

1. ここでIK elementsのところに出ている項目（下の例ではlTip）を選択する。そして，その下のIK element propertiesのElement is activeのチェックボックスにチェックを入れる。

• V- REPでは各関節の動作範囲を指定しておくこと。ただし今回使用するRCサーボモータは０度から１８０度が動作範囲なので，必要な辻盆動作が出来るよう，０度の位 置を決定する必要がある。モータの0度とロボットの角度がずれている場合は，ずれ分をオフセットとして，ｃｓｖファイルを作成する際に引き去っておく必要 がある。

• 下の図（Scene hierarchyでJointをダブルクリックすると出てくるダイアログ）のJoint configurationのPosition is cyclicのチェックを外す。その下のPosition minimumを0.0とし，Position rangeを180.0とする。さらに下のPositionがシミュレータでの初期姿勢の値となる。実際のロボットと合うようにすること。

20msデータ作成方法（教示されたデータをArduinoで実行する20msのデータに展開）

• • 動作教示で出来たファイルをExcelで開き，１列目にそのポーズに移動するのに必要な時間（秒単位，小数可）を挿入し，traj.csvという名前で保存する。

    • • 動作教示で出来たファイル

• • • • 作成するファイルtraj.csv （１列目に右側の関節位置に移動するまでにかける時間（秒）を記入する）

• • シミュレーションを実行する（Child ScriptでrunMode=1とする。またシミュレーション実行はリアルタイム実行ボタンを押しておくこと）。

• 一番右がリアルタイム実行ボタン。左がシミュレーション実行ボタン，止めるときは四角のボタン。

• 動作がおかしくないか確認する。修正の必要があれば，動作間の時間や位置をtraj.csvファイルで修正し，シミュレーション実行(runMode=1)に戻る。

• おかしくなければこれで終了。下記5.を実行する。

• traj.csvと同じフォルダ内（パス名には日本語が入らないようにすること）でmake20msTraj.exeを実行すると，20ms毎のデータがrobot.csvとして作成される。

●Child Script（CoppeliaSim ファイル内にあるLuaプログラム）

example.ttt

（このファイルをダウンロードし，CoppeliaSim ファイルとして立ち上げて下さい。みなさんの環境に応じて変更が必要なところは下記の赤字部分です。）

------------------------------------------------------------------------------

-- Following few lines automatically added by V-REP to guarantee compatibility

-- with V-REP 3.1.3 and later:

if (sim_call_type==sim.syscb_init) then

sim.setScriptAttribute(sim.handle_self,sim.childscriptattribute_automaticcascadingcalls,false)

end

if (sim_call_type==sim.syscb_cleanup) then

end

if (sim_call_type==sim.syscb_sensing) then

sim.handleChildScripts(sim_call_type)

end

if (sim_call_type==sim.syscb_actuation) then

if not firstTimeHere93846738 then

firstTimeHere93846738=0

end

sim.setScriptAttribute(sim.handle_self,sim.scriptattribute_executioncount,firstTimeHere93846738)

firstTimeHere93846738=firstTimeHere93846738+1

------------------------------------------------------------------------------

if (sim.getScriptExecutionCount()==0) then

runMode=0 -- 0:record, 1:play back　　データを記録する時と，シミュレーションで動かす時とで0と1を書き換える。

— 軸の数をここにセット

dof = 7

joint = {}

— 赤はCoppeliaSim内に指定したジョイントの名前。軸数分セットする。７で足りなければjoint[8]などを作る。

joint[1]=simGetObjectHandle(“weist_joint“) — Handle of the waist

joint[2]=simGetObjectHandle(“Ljoint“) — Handle of the left shoulder joint

joint[3]=simGetObjectHandle(“Rjoint“) — Handle of the right shoulder joint

joint[4]=simGetObjectHandle(“LAMjoint“) — Handle of the left upper arm joint

joint[5]=simGetObjectHandle(“RAMjoint“) — Handle of the right upper arm joint

joint[6]=simGetObjectHandle(“LAMjoint1“) — Handle of the left lower arm joint

joint[7]=simGetObjectHandle(“RAMjoint1“) — Handle of the right lower arm joint

— joint[8]=simGetObjectHandle(“RAMjoint1“) — Handle of the right lower arm joint

— joint[9]=simGetObjectHandle(“RAMjoint1“) — Handle of the right lower arm joint

ctrl=simGetUIHandle("UI")

simSetUIButtonLabel(ctrl,0,"getData")

if (runMode == 0) then

jj = {}

— runMode=0のときのデータを書き出すファイル名（フルパス指定）

f= io.open(“C:/Users/Kikai/Desktop/writeMW.csv”,”w”)

if dof == 5 then

f:write("j1, j2, j3, j4, j5\n")

elseif dof == 6 then

f:write("j1, j2, j3, j4, j5, j6\n")

elseif dof == 7 then

f:write("j1, j2, j3, j4, j5, j6, j7\n")

elseif dof == 8 then

f:write("j1, j2, j3, j4, j5, j6, j7, j8\n")

elseif dof == 9 then

f:write("j1, j2, j3, j4, j5, j6, j7, j8, j9\n")

end

else

for i = 1, dof do

sim.setJointMode(joint[i], sim.jointmode_kinematic, 0)

end

changeTiming=0

jointP={}

pJointP={}

dJoint={}

for i = 1, dof do

jointP[i]=0

end

deg2rad=math.pi/180

–動作を確認するCSVファイル名。

motionFile = io.input(“C:/Users/Kikai/Desktop/traj.csv“) –for win

simulationFlag = true

end

rad2deg=180/math.pi

end

sim.handleChildScripts(sim_call_type)

buttonHandle,values=simGetUIEventButton(ctrl)

if (runMode == 0) then

if (buttonHandle==3) then

--print(buttonHandle)

— 軸数に合わせて増減して下さい。下記は７軸の例

jj[1]=sim.getJointPosition(joint[1])*rad2deg

jj[2]=sim.getJointPosition(joint[2])*rad2deg

jj[3]=sim.getJointPosition(joint[3])*rad2deg

jj[4]=sim.getJointPosition(joint[4])*rad2deg

jj[5]=sim.getJointPosition(joint[5])*rad2deg

jj[6]=sim.getJointPosition(joint[6])*rad2deg

jj[7]=sim.getJointPosition(joint[7])*rad2deg

print(jj[1]..", "..jj[2]..", "..jj[3]..", "..jj[4]..", "..jj[5]..", "..jj[6]..", "..jj[7].."\n")

f:write(jj[1]..", "..jj[2]..", "..jj[3]..", "..jj[4]..", "..jj[5]..", "..jj[6]..", "..jj[7].."\n")

–８軸だったらjj[8]を追加するとともに，下記のように書き換えて下さい。

— jj[8]=simGetJointPosition(joint[8])*rad2deg

— f:write(jj[1]..”, “..jj[2]..”, “..jj[3]..”, “..jj[4]..”, “..jj[5]..”, “..jj[6]..”, “..jj[7]..”, “..jj[8]..”\n”)

end

if (sim.getSimulationState()==sim.simulation_advancing_lastbeforestop) then

f:close()

-- Put some restoration code here

end

else -- runMode = 1: play back

currentTime=sim.getSimulationTime()

if(currentTime >= changeTiming) then

tf = true

while tf do

line = io.read()

if line == nil then

simulationFlag = false

sim.stopSimulation()

break

end

if(string.sub(line,1,1)~="#") then

tf = false

--result=sim.auxiliaryConsolePrint(consoleHandle,line)

for i = 1, dof do

pJointP[i] = jointP[i]

end

if dof == 5 then

t, jointP[1], jointP[2], jointP[3], jointP[4], jointP[5]=string.match(line,"(.-)%,(.-)%,(.-)%,(.-)%,(.-)%,(.+)")

end

if dof == 6 then

t, jointP[1], jointP[2], jointP[3], jointP[4], jointP[5], jointP[6]=string.match(line,"(.-)%,(.-)%,(.-)%,(.-)%,(.-)%,(.-)%,(.+)")

end

if dof == 7 then

t, jointP[1], jointP[2], jointP[3], jointP[4], jointP[5], jointP[6], jointP[7]=string.match(line,"(.-)%,(.-)%,(.-)%,(.-)%,(.-)%,(.-)%,(.-)%,(.+)")

end

if dof == 8 then

t, jointP[1], jointP[2], jointP[3], jointP[4], jointP[5], jointP[6], jointP[7], jointP[8]=string.match(line,"(.-)%,(.-)%,(.-)%,(.-)%,(.-)%,(.-)%,(.-)%,(.-)%,(.+)")

end

if dof == 9 then

t, jointP[1], jointP[2], jointP[3], jointP[4], jointP[5], jointP[6], jointP[7], jointP[8], jointP[9]=string.match(line,"(.-)%,(.-)%,(.-)%,(.-)%,(.-)%,(.-)%,(.-)%,(.-)%,(.-)%,(.+)")

end

-- t, waistP, lShoulderP, rShoulderP, lUArmP, rUArmP, lLArmP, rLArmP=string.match(line,"(.-)%,(.-)%,(.-)%,(.-)%,(.-)%,(.-)%,(.-)%,(.+)")

-- result=sim.auxiliaryConsolePrint(consoleHandle,t..', '..waistP..', '..lShoulderP..', '..rShoulderP..', '..lUArmP..', '..rUArmP..', '..lLArmP..', '..rLArmP.."\n")

else

-- result=sim.auxiliaryConsolePrint(consoleHandle,line.."\n")

end

end

if simulationFlag then

t=tonumber(t)

if(t==0) then

--sim.setJointPosition(leftLowerArm, 0*deg2rad)

result=sim.stopSimulation()

end

steps=t/sim.getSimulationTimeStep()

for i = 1, dof do

jointP[i] = tonumber(jointP[i])

dJoint[i] = (jointP[i] - pJointP[i])/steps

end

--print(t..', '..lUArmV..', '..rUArmV..', '..lLArmV..', '..rLArmV)

--result=sim.auxiliaryConsolePrint(consoleHandle,t..', '..lu..', '..ru..', '..ll..', '..rl)

changeTiming=currentTime+t

end

end

if simulationFlag then

for i = 1, dof do

pJointP[i] = pJointP[i] + dJoint[i]

sim.setJointPosition(joint[i],pJointP[i]*deg2rad)

end

-- f:write(pwaistP..", "..plShoulderP..", "..prShoulderP..", "..plUArmP..", "..prUArmP..", "..plLArmP..", "..prLArmP.."\n")

end

if (sim.getSimulationState()==sim.simulation_advancing_lastbeforestop or not simulationFlag) then

-- Put some restoration code here

motionFile:close()

-- f:close()

-- sim.auxiliaryConsolePrint(consoleHandle,"*** simulation end ***\n")

end

end

------------------------------------------------------------------------------

-- Following few lines automatically added by V-REP to guarantee compatibility

-- with V-REP 3.1.3 and later:

end

------------------------------------------------------------------------------