読者です 読者をやめる 読者になる 読者になる

szmlb.net

tips for robotics

V-REPメモ (関節制御)

V-REPで用意されているロボットモデルを制御する方法は, tutorialsにまとめられている.

Writing code in and around V-REP

基本的にはLuaで記述し, ClientサイドからC++pythonといった言語でAPIを呼ぶことも可能らしい. また, ROS連携も可能なので, 任意の言語で開発しROS経由でpub/subすることも可能なようだ.

以下, 本家のサイトに開発方法とV-REP全体のframeworkがまとめられている.

http://www.coppeliarobotics.com/helpFiles/en/images/controlEntities.jpg

http://www.coppeliarobotics.com/helpFiles/en/images/writingCode1.jpg

Luaで関節を制御するスクリプトを書く

Luaでロボットを制御するスクリプトを理解していく.
APIのリストはココ
V-REP API framework
Regular APIを使う.

・関節空間でのPoint to point motion planning

まず, 用意されているロボットの制御用スクリプトがどのように記述されているか見てみた.
KUKA LBR4+をSceneに読み込んでスクリプトを確認.
f:id:szmlb:20140913153508j:plain

-- This is a threaded script, and is just an example!
simSetThreadSwitchTiming(2) 
simDelegateChildScriptExecution()
jointHandles={-1,-1,-1,-1,-1,-1,-1}

for i=1,7,1 do
	jointHandles[i]=simGetObjectHandle('LBR4p_joint'..i)
end

-- Set-up some of the RML vectors:
vel=110
accel=40
jerk=80
currentVel={0,0,0,0,0,0,0}
currentAccel={0,0,0,0,0,0,0}
maxVel={vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180}
maxAccel={accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180}
maxJerk={jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180}
targetVel={0,0,0,0,0,0,0}

targetPos1={90*math.pi/180,90*math.pi/180,170*math.pi/180,90*math.pi/180,90*math.pi/180,90*math.pi/180,0}
simRMLMoveToJointPositions(jointHandles,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,targetPos1,targetVel)

targetPos2={-90*math.pi/180,90*math.pi/180,180*math.pi/180,90*math.pi/180,90*math.pi/180,90*math.pi/180,0}
simRMLMoveToJointPositions(jointHandles,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,targetPos2,targetVel)

targetPos3={0,0,0,0,0,0,0}
simRMLMoveToJointPositions(jointHandles,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,targetPos3,targetVel)

ここで呼ばれているRegular APIはsimRMLMoveToJointPositions()という関数.
V-REPでは Reflexxes Motion Library(RML)というmotion planning用のライブラリを使用しており, この関数もRMLを使用している.
スクリプトでは各軸の速度・加速度・ジャークの最大値を指定し, 現在地から指定した目標関節角度・角速度に到達するよう制御する.
時間の指定がないところを見ると, 設定したパラメータに基づいて目標位置に到達するようplanningされるようだ.
上記のスクリプトでは, 3種類の目標角度を設定し, 順にその角度に到達するよう記述されている.
targetPosxの数値を変更することで, 目標関節角度の変更が可能.

・関節空間での目標関節角度・角速度を各ステップごとに指定する

もうすこし低レイヤーな制御APIがないか探してみた.
各ステップごとにロボットの状態を取得し, 各軸の速度/位置指令を更新するスクリプトを記述するにはどうれすればよいのだろうか.

motion planning用APIを使わずに, 各ステップごとに関節の速度や位置を設定するには,
simSetJointTargetVelocity(), simSetJointTargetPosition()を利用する.

下記のtutorialでは, ロボットを制御するためのそれぞれの方法の説明されている.
BubbleRob tutorial
Luaスクリプトのサンプルスクリプトは, VREPフォルダのscenes/controlTypeExamples.tttを開き, object名:scriptControlledBubbleRobのチャイルドスクリプトから確認できる.

simSetThreadSwitchTiming(200) -- We wanna manually switch for synchronization purpose (and also not to waste processing time!)
simDelegateChildScriptExecution()

-- Get some handles first:
local leftMotor=simGetObjectHandle("scriptControlledBubbleRobLeftMotor") -- Handle of the left motor
local rightMotor=simGetObjectHandle("scriptControlledBubbleRobRightMotor") -- Handle of the right motor
local noseSensor=simGetObjectHandle("scriptControlledBubbleRobSensingNose") -- Handle of the proximity sensor

-- Add a banner:
black={0,0,0,0,0,0,0,0,0,0,0,0}
orange={0,0,0,0,0,0,0,0,0,1,0.5,0.2}
simAddBanner("I am controlled by an embedded child script!",0,sim_banner_bitmapfont+sim_banner_overlay,nil,simGetObjectAssociatedWithScript(sim_handle_self),black,orange)

local driveBackStartTime=-99
local motorSpeeds={0,0}

-- Following is the control loop:
while (simGetSimulationState()~=sim_simulation_advancing_abouttostop) do
	local result=simReadProximitySensor(noseSensor) -- read the proximity sensor
	if (result>0) then
		-- We detected something. We drive back for 3 seconds while slightly turning:
		simSetJointTargetVelocity(leftMotor,-math.pi*0.5)
		simSetJointTargetVelocity(rightMotor,-math.pi*0.25)
		simWait(3)
	end

	-- Default behaviour is to drive straight:
	simSetJointTargetVelocity(leftMotor,math.pi)
	simSetJointTargetVelocity(rightMotor,math.pi)

	-- Now don't waste time in this loop if the simulation time hasn't changed! This also synchronizes this thread with the main script
	simSwitchThread() -- This thread will resume just before the main script is called again

end

上記のスクリプトはロボットを制御するための雛形のスクリプトとして利用できそうだ.

まず, ロボットの角速度を制御するためのスクリプトを作成する.
先程のKUKA LBR4+のスクリプトを下記のように変更.

simSetThreadSwitchTiming(2) 
simDelegateChildScriptExecution()

local driveBackStartTime=-99
local jointSpeeds={0,0,0,0,0,0,0}
jointHandles={-1,-1,-1,-1,-1,-1,-1}

for i=1,7,1 do
	jointHandles[i]=simGetObjectHandle('LBR4p_joint'..i) --get handles for each joints
end

-- Following is the control loop:
while (simGetSimulationState()~=sim_simulation_advancing_abouttostop) do
	
	local time_passed = simGetSimulationTime()    --time
	local time_step = simGetSimulationTimeStep()  --time step

	for i=1,7,1 do
		simSetJointTargetVelocity(jointHandles[i], math.pi*0.5) -- set joint angular velocity
	end

	simSwitchThread() -- switch to other child script
end

上記のスクリプトは, 各軸の目標角速度をtex:{\displaystyle \frac{\pi}{2}}に設定している.
ここで, joint object側の設定で注意が必要.
joint objectのControl parametersにあるControl loop enabledは, simSetJointTargetVelocity()関数を利用する場合はチェックを外す.
スクリプトからこの設定を変更することも可能だと思われるが見当たらなかったので手動でチェックをはずした.

f:id:szmlb:20140913225437j:plain

シミュレーションを実行すると, 各軸が指定した角速度で回転し, 角度上限に達したところで停止する.

次に, simSetJointTargetPosition()関数を利用して角度を制御する場合のスクリプトは以下の通り.

simSetThreadSwitchTiming(2) 

simDelegateChildScriptExecution()

local driveBackStartTime=-99
local pos_cmd={0,0,0,0,0,0,0}
local vel_cmd={0,0,0,0,0,0,0}

jointHandles={-1,-1,-1,-1,-1,-1,-1}

for i=1,7,1 do

	jointHandles[i]=simGetObjectHandle('LBR4p_joint'..i) --get handles for each joints

end

-- Following is the control loop:
while (simGetSimulationState()~=sim_simulation_advancing_abouttostop) do
	
	local time_passed = simGetSimulationTime()    --time
	local time_step = simGetSimulationTimeStep()  --time step

	for i=1,7,1 do
		pos_cmd[i] = 0.2 * math.sin(2.0 * math.pi * 0.25 * time_passed)
		simSetJointTargetPosition(jointHandles[i], pos_cmd[i]) -- set joint angle
	end

	simSwitchThread() -- switch to other child script
end


角度を制御する時は, PID制御器のゲインを調整可能である.
また, 位置制御時に利用されているPID制御器のスクリプトをカスタマイズすることも可能.

まとめ

(1) ロボットの関節角はRMLを利用してPoint to pointで指定が可能
(2) 関節の角度・角速度はAPIを利用して設定が可能. この時, joint objectの設定に注意が必要

今後はIK→各種センサの利用法理解→データビジュアライゼーション→ROSからの利用の順でまとめていきたい