Robotic Visual Servoing. & RTX robot control in Matlab. RTX control in Matlab. Robotic Visual Servoing Overview. Robotic Visual Servoing Reminder

Size: px

Start display at page:

Download "Robotic Visual Servoing. & RTX robot control in Matlab. RTX control in Matlab. Robotic Visual Servoing Overview. Robotic Visual Servoing Reminder"

Sandra O’Brien’
5 years ago
Views:

1 Robotic Visual Servoing Overview Robotic Visual Servoing Problem : guide a robot to a given target based on visual sensing of the environment & RTX robot control in Matlab Advanced Vision Lecture 12 toby.breckon@ed.ac.uk Computer Vision Lab. Institute for Perception, Action & Behaviour School of Informatics Solution : closed loop visual control (sub problem : noise) RTX Robot Control 1 RTX Robot Control 2 RTX control in Matlab Robotic Visual Servoing Reminder Example : placing tool tip on target point Goal state : tool tip to target distance 0 State error : distance of tool tip to target distance Joint level control Algorithm : Estimate state error = tool tip to target distance (in 2D image) while (state error > T pixels) specify absolute gripper / joint positions relative gripper / joint movements use special Matlab command interface estimate Jacobean compute joint movement angles (in 3D world) move robot joints fraction of movement angles (in 3D world) re estimate state error = tool tip to target distance (in 2D image) RTX Robot Control 3 software available from course web page level of control specify pose of gripper tip in robotic work cell (world co ordinates of robot) (5 d.o.f arm) software will resolve pose as set of joint movements RTX Robot Control 4

RTX robot Matlab Interface Linux OS Platform MATLAB FIFO FIFO Working with the RTX Robots Monitor (Background Process)

release power on RTX (wall + lower switches) push green button (upper switch) stand clear! in working directory enter.

RTX Robot Control 5 need software copied to current directory starts monitor, matlab and calibrates RTX (~2 min.

(on/off 1/0) (on/off green/red) [X,Y,Z,YW,P,R] = command(.

2 RTX robot Matlab Interface Linux OS Platform MATLAB FIFO FIFO Working with the RTX Robots Monitor (Background Process) RS 232 Serial Link To initialize the robot for use: connect to PC with serial cable twist emergency stop button to release power on RTX (wall + lower switches) push green button (upper switch) stand clear! in working directory enter./rtx.sh Monitor program handles Matlab RTX comms. RTX Robot Control 5 need software copied to current directory starts monitor, matlab and calibrates RTX (~2 min.) when ready, awaits matlab commands RTX Robot Control 6 RTX Setup Lower Switch RTX Command Overview Upper Switches (on/off 1/0) (on/off green/red) [X,Y,Z,YW,P,R] = command(...) Emergency Stop Side panel of RTX Serial Port RTX commands of the form: Linux PC RTX Robot Control 7 X : gripper X position Y : gripper Y position Z : gripper Z position YW : gripper yaw angle P : gripper pitch angle R : gripper roll angle RTX Robot Control 8

RTX Absolute Motion RTX Relative Motion Requirement : position gripper at point (x,y,z)' Requirement : position gripper

abs_position_rtx(ta,tp,x,y,z,yw,p,r) moves gripper to position given by: X,Y,Z,Y,P,R Ta allowable angular error after movement

offset rel_angle_rtx(ta,tp,del,dsh,dz,dy,dp,dr) abs_angle_rtx(ta,tp,el,sh,z,y,p,r) sets joints to given parameters increments

RTX Standard Positions open_grippers_rtx(gap) opens pinch gripper to Gap" mm Standard Home Position for RTX rtx_home()

3 RTX Absolute Motion RTX Relative Motion Requirement : position gripper at point (x,y,z)' Requirement : position gripper relative to current position in terms of (x,y,z) and/or yaw/pitch/roll specified in world co ordinates abs_position_rtx(ta,tp,x,y,z,yw,p,r) moves gripper to position given by: X,Y,Z,Y,P,R Ta allowable angular error after movement Tp allowable position error after movement rel_position_rtx(ta,tp,dx,dy,dz,dyw,dp,dr) increments gripper position by given offset rel_angle_rtx(ta,tp,del,dsh,dz,dy,dp,dr) abs_angle_rtx(ta,tp,el,sh,z,y,p,r) sets joints to given parameters increments joint parameters by given value (more complex,not recommed here) RTX Robot Control 9 RTX Robot Control 10 RTX Gripper Control RTX Standard Positions open_grippers_rtx(gap) opens pinch gripper to Gap" mm Standard Home Position for RTX rtx_home() close_grippers_rtx() close completely (beware of crushing objects!) RTX stowed position rtx_exit() always use when finished RTX Robot Control 11 RTX Robot Control 12

4 RTX Error Detection & Correction Emergency stop: % Warm calibrate and return to home [x,y,z,yaw,pitch,roll] = calibrate_rtx(0) [x,y,z,yaw,pitch,roll] = rtx_home if heading out of range or towards a fixed object post stop : reset e stop, power up, recalibrate rtx_home % Open the grippers to pick up the block open_grippers_rtx(89) % Move to (x,y,z,yaw,pitch,roll) [x1,y1,z1,yaw1,pitch1,roll1] = abs_position_rtx(4,4,500,0,400,0,95,0) % Move down 350 to pick up block [x2,y2,z2,yaw2,pitch2,roll2] = rel_position_rtx(4,4,0,0,-350,0,0,0); Calibration: RTX Control Example 1 (demo.m) % Close grippers by 10 to pick up block open_grippers_rtx(54) calibrate_rtx(n) N=0 returns to home position % Get angle location [elbow,shoulder,zed,yaw,pitch,roll] = location_an_rtx N=1 recalibrates joints and returns to home position % Go to new location 300 up [elbow,shoulder,zed,yaw,pitch,roll] =... abs_angle_rtx(4,4,elbow,shoulder,zed+300,yaw,pitch,roll) If RTX hangs or is unresponsive then power off/on, recalibrate, rtx_home() RTX Robot Control 13 % Change elbow orientation [elbow,shoulder,zed,yaw,pitch,roll] = rel_angle_rtx(4,4,50,0,0,0,0,0) RTX Robot Control 14 RTX Control Example 2 (demo.m) % Get position and do a relative position movement [x,y,z,yaw,pitch,roll] = location_xy_rtx [x,y,z,yaw,pitch,roll] = abs_position_rt (4,4,x,y,z,yaw,pitch,roll+20) Visual Servoing in Matlab Visual servoing code outline : % Move down 300 to release up block [x,y,z,yaw,pitch,roll] = rel_position_rtx(4,4,0,0,-300,0,0,0) % Release block and close grippers open_grippers_rtx(89) [x,y,z,yaw,pitch,roll] = rel_position_rtx(4,4,0,0,350,0,0,0) close_grippers_rtx estimate initial distance between gripper & target while distance > threshold estimate Jacobean % Cold calibrate to finish (reset to home) [x,y,z,yaw,pitch,roll] = calibrate_rtx(1) move robot joint based on Jacobean % Exit - closes matlab, stows robot rtx_exit re estimate distance between gripper & target RTX Robot Control 15 RTX Robot Control 16

5 Matlab : Visual Servoing Outline % initialize RTX position [NX,NY,NZ,NY,NP,NR] = calibrate_rtx(0); [NX,NY,NZ,NY,NP,NR] = abs_position_rtx(4,4,400,0,400,0,90,0); Lecture Question % get initial image and separation binimage=getbinimage(2,0,1,5); del2=finddist(binimage,fig6); What could go wrong with the thresholding and visual servoing approach? Robot Effector % loop until close enough while del2 > 5 % estimate Jacobean (here simplify to 1D - vertical move) J = estjacob(binimage,7) ; deltaz = del2/j; % move half of the distance [NX,NY,NZ,NY,NP,NR] =... rel_position_rtx(4,4,0,0,-deltaz/2,0,0,0); Target % get new image and separation binimage=getbinimage(0,0,0,5); del2=finddist(binimage,0); RTX Robot Control 17 RTX Robot Control 18 Sub problem : distance between two regions Sub problem : distance between two regions % finds the horizontal distance between the two blobs in the image Continued... function del2 = finddist(bimage) labs=mybwlabel(bimage,4); % get regions % make sure that there are at least 2 big regions if stats(1).area < 100 stats(2).area < 100 del2 = 0; return; % get region properties, make sure there are at least 2 regions stats = regionprops(labs,['basic']); [N,W] = size(stats); if N < 2 del2 = 0; return ; % do bubble sort on regions in case there are more than 2 for i = 1 : N-1 for j = i+1 : N if stats(i).area < stats(j).areatmp = stats(i); stats(i) = stats(j); stats(j) = tmp; RTX Robot Control 19 % get left and right boxes of 2 largest lc = stats(1).centroid; rc = stats(2).centroid; lbb = stats(1).boundingbox; rbb = stats(2).boundingbox; if lc(1) > rc(1) tmp = lbb; lbb = rbb; rbb = tmp; % get distance between left edge of right box and right edge of left box del2= rbb(1) - (lbb(1)+lbb(3)); RTX Robot Control 20

Visual Servoing Results 1 Sub problem : estimate Jacobean % estimate the Jacobean,

tip % find leftmost edge of rightmost blob in current image edgebefore =

[NX,NY,NZ,NY,NP,NR]=rel_position_rtx(4,4,0,0,10,0,0,0); Target

Image edgeafter = findlredge(newbinimage); % return to original position and compute

(edgeafter-edgebefore)/10; Camera Viewpoint Histogram RTX Robot Control 21 RTX Robot

.. closed loop visual control perception / action noise model based approach Vs.

6 Visual Servoing Results 1 Sub problem : estimate Jacobean % estimate the Jacobean, here in 1D for simplicity function J = estjacob(binimage) Example setup & images Tool tip % find leftmost edge of rightmost blob in current image edgebefore = findlredge(binimage); % move up 10 mm and get new image [NX,NY,NZ,NY,NP,NR]=rel_position_rtx(4,4,0,0,10,0,0,0); Target newbinimage=getbinimage(0,0,0,0); % find leftmost edge of new rightmost blob Camera Image edgeafter = findlredge(newbinimage); % return to original position and compute Jacobean [NX,NY,NZ,NY,NP,NR]=rel_position_rtx(4,4,0,0,-10,0,0,0); J = (edgeafter-edgebefore)/10; Camera Viewpoint Histogram RTX Robot Control 21 RTX Robot Control 22 Visual Servoing Results 2 Summary Visual Servoing... closed loop visual control perception / action noise model based approach Vs. visual vervoing Algorithm Outline Example : Visual Servoing Visual Servoing (in 1D) : 5 iterations required RTX Robot Control 23 Jacobean Estimation RTX Robot Control & Usage Matlab examples RTX Robot Control 24

Advanced Vision Guided Robotics. David Bruce Engineering Manager FANUC America Corporation

Advanced Vision Guided Robotics David Bruce Engineering Manager FANUC America Corporation Traditional Vision vs. Vision based Robot Guidance Traditional Machine Vision Determine if a product passes or