TUTORIAL T4: Visual servoing using Matlab

Based on chapter 15 of Robotics, Vision and Control by Peter Corke.

It uses the The machine vision toolbox.

The machine vision toolbox manual is a pdf document auto-generated from the comments in the MATLAB code.

The Machine Vision Toolbox

Toolbox utilities

The Machine Vision Toolbox (MVTB) provides many functions that are useful in machine vision and vision-based control. They include:

• Camera models: CentralCamera, SphericalCamera,…
• Spatial operators: linear convolution, kernels, non-linear, morphological, similarity,…
• Features: region features, line features, point features,…
• Multiview: Geometric, stereo, image sequences,…
• Utilities and plotting

Just a very small subset will be used here.

The camera: The central projection perspective model

Default camera

Create an instance of the default camera to see all its properties:

>> cam = CentralCamera('default')

These properties can also be individually accessed, e.g.:

>> cam.f
>> cam.pp
>> cam.rho

Plotting the image

Function plot obtains and plots the pixel coordinates of a 3D point defined w.r.t. the world frame (by default the camera frame coincides with the world frame and the object frame). As an example, the point P shown in the previous figure is plotted in the image plane as follows:

>> P = [1.0,-1.0,4.0]';
>> p1 = cam.plot(P)

This can be repeated by translating the camera frame or the object frame using the function project that has the following options:

• ‘pose’,T: Set the camera pose to the homogeneous transformation T before projecting points to the camera image plane. Temporarily overrides the current camera pose C.T.
• ‘objpose’,T: Transform all points by the homogeneous transformation T before projecting them to the camera image plane.

Translate the object to the negative y-direction:

>> p2=cam.project(P,'objpose',transl(0,-1,0))
>> cam.plot([p1,p2])

Translate the camera (temporarily) to the negative x-direction:

>> p3=cam.project(P,'pose',transl(-1,0,0))
>> cam.plot([p1,p3])

The camera transformation can be set fixed, not only for the current call of the project function:

>> cam.T = transl(-1,0,0)
>> p4=cam.project(P)
>> cam.plot([p1,p4])

Examples: feature extraction using a hand-held camera

Example 1

Execute the camera.m script to determine and visualize the image features of the four vertexes of the top face of a cube.

The following auxiliar files are required to draw the cube and the image plane: cube_plot.m and plotCameraImagePlane.m

%% CUBE OBJECT DEFINITION
%Cube size; origin at a vertex.
sizex=.8;
sizey=.8;
sizez=.5;
% Definition of 3D points w.r.t. cube frame
P1 = [sizex,  sizey, 0]';
P2 = [sizex,  0, 0]';
P3 = [0,  sizey, 0]';
P4 = [0,  0, 0]';
P = [P1 P2 P3 P4]

% Setting object frame (with orientation defined by consecutive rotations
%around fixed axis x,y,z)
Tobject = transl(0, -1, sizez)*rpy2tr(0,0,-10*pi/180);

%% CAMERA SETUP
%
cam = CentralCamera('default');
cam.f=0.008; %default is 0.008
% setting camera frame (with orientation defined by consecutive rotations
%around fixed axis x,y,z)
cam.T = transl(0, 0, 3)*rpy2tr(0, 160*pi/180, 10*pi/180);
p = cam.plot(P, 'objpose', Tobject ) %with object translated

%% VISUALIZE 3D FRAMES
%
figure
% plot world frame
W = transl(0,0,0);
trplot(W, 'frame', 'W', 'axis', [-3 3 -3 3 -2 4], 'view', [-60 10])
hold on
% plot camera frame
trplot(cam.T, 'frame', 'C', 'color', 'k')
% plot object frame
trplot(Tobject, 'frame', 'O', 'color', 'r')
% plot Pw, i.e. point P w.r.t. world frame
% Pw=Tobject*[P;1];
Pw=Tobject*cat(1,P,ones(1,size(P,2)));
plot3(Pw(1,:),Pw(2,:),Pw(3,:),'*r')

% Plot the camera and the virtual image plane
zplane = 1.5;
scalecamera = 0.3;
plotCameraImagePlane(cam, zplane, Pw, scalecamera);

%plot the object
cube_plot(Tobject,sizex,sizey,sizez,'r');

Example 2

Modify the camera.m script to consider:

• The cube located at Tobject=transl(1,1,0) and the camera looking perpendicularly at the top face of the cube, centered at the face, and at a distance of 1.5.

Example 3

Execute the puma560_camera.m script to determine and visualize the image features the four vertexes of the top face of a cube.

%% CUBE OBJECT DEFINITION
%Cube size; origin at a vertex.
sizex=.08;
sizey=.08;
sizez=.05;
% Definition of 3D points w.r.t. cube frame
P1 = [sizex,  sizey, 0]';
P2 = [sizex,  0, 0]';
P3 = [0,  sizey, 0]';
P4 = [0,  0, 0]';
P = [P1 P2 P3 P4]
% Setting object frame (with orientation defined by consecutive rotations
%around fixed axis x,y,z)
Tobject = transl(0.5, 0, sizez)*rpy2tr(0,0,-10*pi/180);
%% ROBOT
% puma560t
mdl_puma560
p560.tool = transl(0, 0, 0.18);%define the camera to be placed ahead the
                               %gripper z-axis
TCP = Tobject*transl(0,0,0.35)*rpy2tr(10*pi/180,160*pi/180,10*pi/180);
q=p560.ikine6s(TCP)
%% CAMERA SETUP
%
cam = CentralCamera('default');
cam.f=0.008; %default is 0.008
% setting camera frame (with orientation defined by consecutive rotations
%around fixed axis x,y,z)
cam.T = TCP;
p = cam.plot(P, 'objpose', Tobject ) %with object translated

%% VISUALIZE THE 3D SCENE
%
figure
% plot world frame
W = transl(0,0,0);
trplot(W, 'frame', 'W', 'axis', [-.4 1 -.6 .6 0 1.0], 'length', 0.2)
hold on
% plot camera frame
trplot(cam.T, 'frame', 'C', 'color', 'k', 'length', 0.1)
% plot object frame
trplot(Tobject, 'frame', 'O', 'color', 'r', 'length', 0.1)
% plot Pw, i.e. point P w.r.t. world frame
% Pw=Tobject*[P;1];
Pw=Tobject*cat(1,P,ones(1,size(P,2)));
plot3(Pw(1,:),Pw(2,:),Pw(3,:),'*r')

% Plot the camera and the virtual image plane
zplane = 0.15;
scalecamera = 0.08;
plotCameraImagePlane(cam, zplane, Pw, scalecamera);

%plot the object
cube_plot(Tobject,sizex,sizey,sizez,'r');
hold on
p560.plot(q, 'noarrow', 'view', [-150 30]) %plot options in page 230 of Robotics toolbox manual

Example 4

Modify the puma560_camera.m script to consider:

• The cube located at Tobject=transl(-0.2,0.2,0) and the camera looking perpendicularly at the top face of the cube and at a distance of 0.4 m.

The poseEstimation script

The poseEstimation.m script implements the pose estimation algorithm detailed in Section Pose Estimation:

function Tc_t_est = poseEstimation(P,p,cam)
%Function that estimates the pose of the object w.r.t. the camera
% from the points P on the object (w.r.t. object frame) and pixels p
% of the camera cam.
% Variable names are set to make it easy to understand following 
% Siciliano's book

%First compute the features si=[Xi,Yi]' from the pixels pi of the camera
% px=f*alphax*X+X0
% p(1,1)=(cam.f/cam.rho(1))*X1+cam.u0
% py=f*alphay*Y+Y0
% p(2,1)=(cam.f/cam.rho(2))*Y1+cam.v0
X1 = (p(1,1)-cam.u0)*(cam.rho(1)/cam.f);
Y1 = (p(2,1)-cam.v0)*(cam.rho(2)/cam.f);

X2 = (p(1,2)-cam.u0)*(cam.rho(1)/cam.f);
Y2 = (p(2,2)-cam.v0)*(cam.rho(2)/cam.f);

X3 = (p(1,3)-cam.u0)*(cam.rho(1)/cam.f);
Y3 = (p(2,3)-cam.v0)*(cam.rho(2)/cam.f);

X4 = (p(1,4)-cam.u0)*(cam.rho(1)/cam.f);
Y4 = (p(2,4)-cam.v0)*(cam.rho(2)/cam.f);

%Coordinates of the points on the object 
rx1 = P(1,1);
ry1 = P(2,1);
rx2 = P(1,2);
ry2 = P(2,2);
rx3 = P(1,3);
ry3 = P(2,3);
rx4 = P(1,4);
ry4 = P(2,4);

% Skew-symetric matrices Si
% Si=[0 -1 -Yi;1 0 -Xi; -Yi Xi 0]
S1=[0 -1 Y1; 1 0 -X1; -Y1 X1 0];
S2=[0 -1 Y2; 1 0 -X2; -Y2 X2 0];
S3=[0 -1 Y3; 1 0 -X3; -Y3 X3 0];
S4=[0 -1 Y4; 1 0 -X4; -Y4 X4 0];

% Ai=[rxi*Si ryi*Si Si]
A1=[rx1*S1 ry1*S1 S1];
A2=[rx2*S2 ry2*S2 S2];
A3=[rx3*S3 ry3*S3 S3];
A4=[rx4*S4 ry4*S4 S4];
A=[A1;A2;A3;A4];

% Solve using the singular value decomposition of A
[U,S,V] = svd(A);
H=[V(1:3,9) V(4:6,9) V(7:9,9)];

%psi with minus sign in order to locate the object in front of the camera,
%i.e. zp>0 
if H(3,3)>0
    sign=1;
else
    sign=-1;
end
psi=sign/norm(H(:,1)); 
H=H.*psi;
r1=H(:,1);
r2=H(:,2);
o=H(:,3);
r3=cross(r1,r2);
%disp('The esimated transform is:')
Tc_t_est=[r1 r2 r3 o; 0 0 0 1];
Tc_t_est=trnorm(Tc_t_est);

Example 5

With the data of exercise 1 (points P and corresponding features p), estimate the pose of the object w.r.t. the camera using the poseEstimation function. Verify the correctness of the result.

Example 6

With the data of exercise 3 (points P and corresponding features p), estimate the pose of the object w.r.t. the frame of base of the robot. Verify the correctness of the result.

Position-based visual servoing algorithm

The problem

Given:

• TcStar_t: the pose of the object w.r.t. the desired camera frame, and
• Tc_t_est: the pose of the object w.r.t. the current camera frame estimated from the image features and the known position of the points in the object frame,

the position-based visual servoing control:

• Computes Tdelta, the error between {C} and {C*}.
• From {C} advances a small step in the direction defined by Tdelta.

Note that the pose of the object w.r.t. the world frame is not required, only the position of the points on the object w.r.t. the object reference frame.

EXERCISES

Exercise 1

Complete the visualServoingLoop.m to move the camera from the initial pose

Tc0 =transl(0, 0, 3)*trotz(0)*troty(160*pi/180)*trotx(10*pi/180)

to a final pose Tc where the object frame measured from the camera frame is:

TcStar_t = troty(pi)*transl(-sizex/2, -sizey/2, -1.5).

Hints:

• Use trinterp function to compute the transform to be used to advance form {C} towards {C*} an small step, i.e. trinterp(A,B,lambda), where A is the identity matrix, i.e. eye(4,4,), B is Tdelta and lambda is a value within [0,1]
• Use trnorm after multiplying to transforms to make sure that the result is a correct homogeneous transform matrix.

Exercise 1b (optional)

Repeat with a different initial configuration such that not all of the four points are visible.

1. Comment the results. What would happen with a real camera?
2. Change the camera parameters in order to make all the points visible

Exercise 2

Repeat exercise 1 but using the visualServoingLoop_p560.m script to move the camera held by the puma560 robot.

Modify the transforms of the TCP w.r.t. the object reference frame at the initial and goal configurations:

2) TobjTCP_ini = transl(-0.4, -0.3, 0.3)*troty(130*pi/180)*trotx(-10*pi/180)

TobjTCP_goal = transl(-0.3, 0.3, 0.4)*troty(160*pi/180);

3) TobjTCP_ini = transl(sizex/2, sizey/2, 0.35)*rpy2tr(10*pi/180, 160*pi/180, 20*pi/180);

TobjTCP_goal = transl(-0.3, 0.3, 0.4)*troty(160*pi/180);

4) TobjTCP_ini = transl(sizex/2, sizey/2, 0.35)*rpy2tr(10*pi/180, 160*pi/180, 20*pi/180);

TobjTCP_goal = transl(sizex/2, sizey/2, 0.8)*troty(pi);

Comment the problems that appear in these examples and discuss possible solutions.

Exercise 2b (optional)

Implement the resolved velocity control: Compute the Jacobian J_{Ad} to compute the joint velocities qdot from the error, considering gain matrix K=0.1.* eye(6). Update the configuration as qcurrent=qcurrent+Delta* qdot, with Delta = 1.