VSL - Visuospatial Skill Learning (Demonstration)
This source code is the implementation of the algorithms described in Chapter 4 of the book “Handling Uncertainty and Networked Structure in Robot Control”, Lucian Busoniu and Levente Tamas (eds.), Springer, 2015.
Author: Reza Ahmadzadeh, 2015
% http://www.ahmadzadeh.info % % The program consists of two phases: demonstration and reproduction. % in the demonstration phase, it loads 3 captured images including a % pre-action, a post-action, and another pre-action from the next action. % The VSL experiment includes (described in the chapter) a pick-and-place % operation in which the tutor picks and places each object in the scene. % The robot observes the operation and capture images. Two observations are % extracted from the images (a pre-action and a post-action observation). % The pre-action observation represents the manipulated object and the % post-action observation represent the place that the object was moved to. % In the demonstration phase, the robot learns the spatial relationship % between objects and their surrounding environment. % % In the reproduction phase the objects are randomized in the scene. Still % the robot can detect the objects and execute a sequence of operations to % reach the goal of the task. % % This source code is given for free! However, I would be grateful if you refer % to the book (or corresponding articles) in any academic publication that uses % this code or part of it. Here are the corresponding BibTex references: % % % @inproceedings{ahmadzadeh2013interactive, % title={Interactive Robot Learning of Visuospatial Skills}, % author={Ahmadzadeh, Seyed Reza and Kormushev, Petar and Caldwell, Darwin. G.}, % booktitle={Advanced Robotics (ICAR) 2013, 16th International Conference on}, % pages={1--8}, % year={2013}, % organization={IEEE} % } % % @inproceedings{ahmadzadeh2013visuospatial, % title={Visuospatial Skill Learning for Object Reconfiguration Tasks}, % author={Ahmadzadeh, Seyed Reza and Kormushev, Petar and Caldwell, Darwin G.}, % booktitle={Intelligent Robots and Systems ({IROS}), 2013 {IEEE/RSJ} International Conference on}, % pages={685--691}, % year={2013}, % organization={IEEE} % } % % % This is a simple demo that represents the implementation of the VSL % algorithm. This also can be considered as a validation of the concept. % % ================================================ % Reza Ahmadzadeh (reza.ahmadzadeh@iit.it) % ================================================ clc,clear, close all; fprintf('VSL - Visuospatial Skill Learning\n'); fprintf('---------------------------------------\n'); fprintf('--- Demonstration ---\n'); fprintf('---------------------------------------\n'); fprintf('For this Demo, we consider that the tutor demonstrates\n'); fprintf('a simple pick-and-place action on a set of objects and\n'); fprintf('the operation in each step is captured by a camera.\n'); % ---------------------------------------------------- % Capturing images from the camera...\n'); % ---------------------------------------------------- % in this demo we are not capturing from camera. Use this section to % capture from the camera. % ---------------------------------------------------- % Reading and drawing the raw images (demonstration phase) % ---------------------------------------------------- preAction1 = imread('img\demonstration\preActionObservation.png'); postAction1 = imread('img\demonstration\postActionObservation.png'); preAction2 = imread('img\demonstration\preActionObservation2.png'); I1 = im2double(preAction1); I2 = im2double(postAction1); I3 = im2double(preAction2); [r,c,~] = size(I1); fprintf('The images are rectified using the homography matrix.\n'); % show the raw images figure;set(gcf,'position',get(0,'ScreenSize')); subplot(1,3,1);imshow(preAction1);title('pre-action observation'); subplot(1,3,2);imshow(postAction1);title('post-action observation'); subplot(1,3,3);imshow(preAction2);title('next pre-action observation'); fprintf('\n As you can see, firstly the A-block has been removed from the scene.\n'); fprintf('and then it has been placed in a new place.\n'); fprintf('To continue press Enter');pause; clear preAction1 postAction1 preAction2 % ---------------------------------------------------- % finding the manipulated object using the pre-action % and post-action observations % ---------------------------------------------------- fprintf('\n\n------\nSTEP-1\n------\n'); fprintf('finding the manipulated object using the \n pre-action and post-action observations\n'); % ----> background subtraction diff1_gray = rgb2gray(abs(I1-I2)); bw1 = im2bw(diff1_gray,graythresh(diff1_gray)); % ----> Calculating the center of mass N = sum(sum(bw1==1)); [rr,cc] = find(bw1==1); xc = fix(sum(rr)/N); yc = fix(sum(cc)/N); % ----> finding the boundaries of the object (not necessary for this Demo) [B,~,~,~] = bwboundaries(bw1,8); [~, max_index] = max(cellfun('size', B, 1)); boundary1 = B{max_index}; % ----> finding the edge of the object (not necessary for this Demo) ed1 = edge(bw1,'sobel',0.025); % ----> extract the object from the scene a = 300; % width of the frame (this can be also determined from the extracted edge information) b = 300; % height of the frame (this can be also determined from the extracted edge information) obj1 = I1(xc-fix(a/2):xc+fix(a/2),yc-fix(b/2):yc+fix(b/2),:); % ----> plot the result fprintf('The object is detected using background subtraction.\n'); figure;set(gcf,'position',get(0,'ScreenSize')); subplot(1,3,1);imshow(I1);title('Pre-action observation'); subplot(1,3,2);imshow(I2);title('Post-action observation'); fprintf('To continue press Enter');pause; subplot(1,3,3);imshow(obj1);title('the manipulated object'); fprintf('\nBesides we can have the edge and the center of the object.\n'); figure;set(gcf,'position',get(0,'ScreenSize')); subplot(1,3,1);imshow(ed1);hold on;plot(boundary1(:,2), boundary1(:,1),'r');title('Edge of the object'); subplot(1,3,2);imshow(bw1);hold on;plot(yc,xc,'+r');title('the object and its center'); subplot(1,3,3);imshow(obj1);title('the picked object'); fprintf('To continue press Enter');pause; clear diff1_gray bw1 ed1 boundary1 max_index xc yc N rr cc B % ---------------------------------------------------- % finding the place that the object is moved to using % the pre-action and the next post-action observations % ---------------------------------------------------- fprintf('\n\n------\nSTEP-2\n------\n'); fprintf('finding the place that the object was moved to \n using post-action and the next pre-action observations\n'); % ----> background subtraction diff1_gray = rgb2gray(abs(I3-I2)); bw1 = im2bw(diff1_gray,graythresh(diff1_gray)); % ----> Calculating the center of mass N = sum(sum(bw1==1)); [rr,cc] = find(bw1==1); xc = fix(sum(rr)/N); yc = fix(sum(cc)/N); % ----> finding the area of the scene the object was placed at I2big = zeros(2*r,2*c,3); I2big(fix(r/2):fix(3*r/2)-1,fix(c/2):fix(3*c/2)-1,:) = I2; place1 = I2big(fix(r/2)+xc-fix(3*a/2):fix(r/2)+xc+fix(3*a/2)-1,fix(c/2)+yc-fix(3*b/2):fix(c/2)+yc+fix(3*b/2)-1,:); figure;set(gcf,'position',get(0,'ScreenSize')); subplot(1,3,1);imshow(I2);title('Post-action observation'); subplot(1,3,2);imshow(I3);title('Next Pre-action observation'); fprintf('To continue press Enter');pause; subplot(1,3,3);imshow(place1);title('the place that the object was placed at'); hold on;plot(fix(3*a/2),fix(3*b/2),'+r'); clear diff1_gray bw1 xc yc N rr cc clear sceneFeatures scenePoints objPairs matchedObjPoints matchedScenePoints objPolygon newObjPolygon TT tform fprintf('\nLearning is finished.\n'); fprintf('We now have the pre-action observation which includes\n'); fprintf('the manipulated object, and the place that the object was\n'); fprintf('moved to. VSL learns the spatial relationship between the object\nand its surrounding context.\n'); fprintf('\n \nNow you should run the reproduction script.\n'); clear I2big inlierBoxPoints inlierScenePoints c r cxx cyy a b
VSL - Visuospatial Skill Learning --------------------------------------- --- Demonstration --- --------------------------------------- For this Demo, we consider that the tutor demonstrates a simple pick-and-place action on a set of objects and the operation in each step is captured by a camera. The images are rectified using the homography matrix. As you can see, firstly the A-block has been removed from the scene. and then it has been placed in a new place. To continue press Enter ------ STEP-1 ------ finding the manipulated object using the pre-action and post-action observations The object is detected using background subtraction. To continue press Enter Besides we can have the edge and the center of the object. To continue press Enter ------ STEP-2 ------ finding the place that the object was moved to using post-action and the next pre-action observations To continue press Enter Learning is finished. We now have the pre-action observation which includes the manipulated object, and the place that the object was moved to. VSL learns the spatial relationship between the object and its surrounding context. Now you should run the reproduction script.
