Commit 886f5e21 authored by Sylvain CALINON's avatar Sylvain CALINON

m_fcts updated

parent e9adb51d
......@@ -50,19 +50,20 @@ x = p * B; %Reconstruction of signal
%% Plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10,10,800,1200]);
figure('position',[10,10,800,800]);
subplot(2,1,1); hold on; axis off;
plot(p(1,:), p(2,:), 'r.','markersize',20);
plot(p(1,:), p(2,:), '.','markersize',30,'color',[.6 .6 .6]);
if nbDeg==3
plot(p(1,[1:2]), p(2,[1:2]), 'r-','linewidth',2);
plot(p(1,[3:4]), p(2,[3:4]), 'r-','linewidth',2);
plot(p(1,[1:2]), p(2,[1:2]), '-','linewidth',2,'color',[.6 .6 .6]);
plot(p(1,[3:4]), p(2,[3:4]), '-','linewidth',2,'color',[.6 .6 .6]);
end
plot(x(1,:), x(2,:), 'k-','linewidth',3);
plot(x(1,:), x(2,:), '-','linewidth',3,'color',[0 0 0]);
subplot(2,1,2); hold on;
for i=0:nbDeg
plot(t, B(i+1,:),'linewidth',3);
end
xlabel('t','fontsize',16); ylabel('b_i','fontsize',16);
set(gca,'xtick',[0,1],'ytick',[0,1],'fontsize',16);
xlabel('t','fontsize',18); ylabel('b_i','fontsize',18);
pause;
close all;
close all;
\ No newline at end of file
......@@ -56,30 +56,32 @@ x2 = phi * w; %Reconstruction
%Alternative computation through vectorization (useful for extending the approach to proMP representations including trajectory covariance)
Psi = kron(phi, eye(nbVar));
w = pinv(Psi) * x(:); %Control points
x2Vec = Psi * w; %Reconstruction
w2 = pinv(Psi) * x(:); %Control points
x2Vec = Psi * w2; %Reconstruction
%% Plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10,10,800,1200]);
subplot(2,1,1); hold on; axis off;
plot(x(:,1), x(:,2), '-','linewidth',2,'color',[.7 .7 .7]);
% plot(w(:,1), w(:,2), '.','markersize',30,'color',[.6 .6 .6]);
plot(x(1,:), x(2,:), '-','linewidth',6,'color',[.7 .7 .7]);
plot(x2(:,1), x2(:,2), 'k-','linewidth',2);
plot(x2Vec(1:2:end), x2Vec(2:2:end), 'r--','linewidth',2);
% plot(w(1,:), w(2,:), 'r.','markersize',20);
subplot(2,1,2); hold on;
for i=0:nbDeg
plot(t,phi(:,i+1));
plot(t, phi(:,i+1), '-','linewidth',3);
end
xlabel('t'); ylabel('b_i');
xlabel('t','fontsize',18); ylabel('b_i','fontsize',18);
set(gca,'xtick',[0,1],'ytick',[0,1],'fontsize',16);
%Plot covariance
figure('position',[1640 10 800 800]); hold on;
colormap(flipud(gray));
pcolor(abs(Psi*Psi'));
set(gca,'xtick',[1,nbData*nbVar],'ytick',[1,nbData*nbVar]);
axis square; axis([1 nbData*nbVar 1 nbData*nbVar]); shading flat;
% %Plot covariance
% figure('position',[1640 10 800 800]); hold on;
% colormap(flipud(gray));
% pcolor(abs(Psi*Psi'));
% set(gca,'xtick',[1,nbData*nbVar],'ytick',[1,nbData*nbVar]);
% axis square; axis([1 nbData*nbVar 1 nbData*nbVar]); shading flat;
pause;
close all;
close all;
\ No newline at end of file
......@@ -78,4 +78,4 @@ end %nb
% print('-dpng','graphs/demo_Bezier_illustr01.png');
pause;
close all;
close all;
\ No newline at end of file
......@@ -163,4 +163,4 @@ axis equal;
% % print('-dpng','graphs/demo_Bezier_highOrder02.png');
pause;
close all;
close all;
\ No newline at end of file
This diff is collapsed.
......@@ -32,29 +32,31 @@ kV = (2*kP)^.5; %Damping in task space
% kV = 10; %Damping in task space
kPq = 10; %Stiffness in joint space
kVq = 1; %Damping in joint space
dt = 0.01; %Time step
nbData = 30;
dt = 1E-2; %Time step
nbData = 100;
%% Create robot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbDOFs = 5; %Number of articulations
armLength = 0.3; %Length of each link
nbDOFs = 2; %Number of articulations
model.l = 0.3; %Length of each link
model.m = 1.7; %Mass
model.g = 9.81; %Acceleration due to gravity
for k=1:nbDOFs
lnk(k).L = Link([0 0 armLength 0], 'standard'); %Links with dh = [THETA D A ALPHA SIGMA OFFSET]
lnk(k).L.m = 1; %Mass
lnk(k).L.r = [-armLength/2 0 0]; %Center of mass
%lnk(k).L.I = zeros(3); %Inertia
lnk(k).L.I = eye(3)*1E-2; %Inertia
lnk(k).L = Link([0 0 model.l 0], 'standard'); %Links with dh = [THETA D A ALPHA SIGMA OFFSET]
lnk(k).L.m = model.m; %Mass
lnk(k).L.r = [-model.l/2 0 0]; %Link COG (center of rod)
lnk(k).L.I = [0; 0; model.m * model.l^2 / 12]; %Inertia at link COG (rod of length L and mass m, https://en.wikipedia.org/wiki/List_of_moments_of_inertia)
lnk(k).L.Jm = 0; %1E6;
lnk(k).L.G = 0;
lnk(k).L.B = 0;
%lnk(k).L.G = 1E-5;
strTmp(k) = lnk(k).L;
lnk(k).rob = SerialLink(strTmp);
lnk(k).rob.gravity = [0 9.81 0]; %for 2D robot
lnk(k).rob.gravity = [0 model.g 0]; %for 2D robot
end
robot = lnk(end).rob;
%Initial pose
q0(:,1) = ones(nbDOFs,1)*pi/4;
Htmp = robot.fkine(q0);
......@@ -72,33 +74,49 @@ dx = zeros(2,1);
dq = zeros(nbDOFs,1);
for t=1:nbData
r.q(:,t) = q; %Log data
Htmp = robot.fkine(q);
x = Htmp.t(1:2);
J = robot.jacob0(q,'trans');
J = J(1:2,:);
tauG = robot.gravload(q')'; %Compute gravity compensation term
uG = robot.gravload(q')'; %Compute torque due to gravity
% uG = cos(q) * model.l * model.m * model.g / 2; %Compute torque due to gravity
%Dynamically consistent nullspace (Jinv=pinv(J) would not work)
JI = robot.inertia(q'); %Joint inertia
CI = J/JI*J' + eye(2)*1E-12; %Cartesian inertia
W = inv(JI);
M = robot.inertia(q') %Joint inertia
% M = model.m * model.l^2 / 3; %Inertia at link frame origin (rod of length L and mass m, https://en.wikipedia.org/wiki/List_of_moments_of_inertia)
Mx = inv(J / M * J' + eye(2)*1E-12); %Cartesian inertia
W = inv(M);
%W = eye(nbDOFs);
Jinv = W * J' / (J*W*J'); %Generalized inverse (weighted pseudoinverse)
Jinv = W * J' / (J*W*J' + eye(2)*1E-12); %Generalized inverse (weighted pseudoinverse)
N = eye(nbDOFs) - J'*Jinv'; %Nullspace projection matrix (dynamically consistent)
%N = (eye(nbDOFs) - Jinv*J)'; %Same as the line above but with Roy Featherston's paper notation
%N = eye(nbDOFs) - pinv(J)*J; %Nullspace projection matrix (for standard least squares solution)
%N = eye(nbDOFs) - J'*pinv(J)'; %Same as the line above (because of symmetry)
%Computed torque control command
% tau = J'/ CI * (ddxh + kP*(xh-x) + kV*(dxh-dx)) + N * JI*(kPq*(qh-q)-kVq*dq) + tauG;
% tau = J'/ CI * (kP*(xh-x)-kV*dx) + N * (kPq*(qh-q)-kVq*dq) + tauG;
% tau = J' * (kP*(xh-x)-kV*dx) + N * (kPq*(qh-q)-kVq*dq) + tauG;
tau = N * (kPq*(qh-q)-kVq*dq) + tauG;
% u = J'* Mx * (ddxh + kP*(xh - x) + kV*(dxh - dx)) + N * M *(kPq * (qh - q) - kVq * dq) + uG;
% u = J'* Mx * (kP * (xh - x) - kV * dx) + N * (kPq * (qh - q) - kVq * dq) + uG;
% u = J' * (kP * (xh - x) - kV * dx) + N * (kPq * (qh - q) - kVq * dq) + uG;
% u = N * (kPq * (qh - q) - kVq * dq) + uG;
% u = uG;
u = rand(nbDOFs,1) * 1E-1 + uG;
% robot.inertia(q')
% uG = robot.gravload(q')'
% ddq = robot.accel(q', dq', u'); %Compute acceleration
ddq = accel(q, u, model);
pause
% ddq = (u - uG) / M; %Compute acceleration
ddq = robot.accel(q', dq', tau');
dq = dq + ddq*dt;
dx = J*dq;
q = q + dq*dt;
dx = J * dq;
end
......@@ -106,11 +124,34 @@ end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[20,10,1800,1200],'color',[1 1 1]); hold on; axis off;
for t=round(linspace(1,nbData,10))
plotArm(r.q(:,t), ones(nbDOFs,1)*armLength, [0;0;t*0.1], .02, [.7 .7 .7]);
plotArm(r.q(:,t), ones(nbDOFs,1)*model.l, [0;0;t*0.1], .02, [.7 .7 .7]-.7*t/nbData);
end
plot(xh(1),xh(2),'r+');
axis([-.2 1 -.2 1]); axis equal; set(gca,'xtick',[],'ytick',[]);
axis equal; %axis([-.2 1 -.2 1]);
%print('-dpng','demoID01.png');
pause;
close all;
\ No newline at end of file
close all;
end
%%%%%%%%%%%%%%%%%%%%%%
%Rollout
function ddq = accel(q, u, model)
for n=1:size(q,1)
M = model.m * model.l^2 / 3 %Inertia at link frame origin (rod of length L and mass m, https://en.wikipedia.org/wiki/List_of_moments_of_inertia)
uG = cos(q(n)) * model.l * model.m * model.g / 2; %Compute torque due to gravity
ddq(n) = (u(n) - uG) / M;
end
end
%%%%%%%%%%%%%%%%%%%%%%
%Jacobian with numerical computation
function J = jacob0(q, model)
e = 1E-6;
J = zeros(2,size(q,1));
for n=1:size(q,1)
qtmp = q;
qtmp(n) = qtmp(n) + e;
J(:,n) = (fkine(qtmp, model) - fkine(q, model)) / e;
end
end
\ No newline at end of file
function demo_IK01
% Inverse kinematics with nullspace.
%
% This demo requires the robotics toolbox RTB10 (http://petercorke.com/wordpress/toolboxes/robotics-toolbox).
% First run 'startup_rvc' from the robotics toolbox.
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
% Written by Sylvain Calinon, http://calinon.ch/
%
% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
%
% PbDlib is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License version 3 as
% published by the Free Software Foundation.
%
% PbDlib is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
addpath('./m_fcts/');
disp('This demo requires the robotics toolbox RTB10 (http://petercorke.com/wordpress/toolboxes/robotics-toolbox).');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dt = 0.01; %Time step
nbData = 100;
%% Robot parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nbDOFs = 3; %Number of articulations
armLength = 0.6; %Length of each link
L1 = Link('d', 0, 'a', armLength, 'alpha', 0);
robot = SerialLink(repmat(L1,nbDOFs,1));
%Initial pose
q(:,1) = [pi/2 pi/2 pi/3]; %ones(1,nbDOFs) .* pi/3;
%% IK with nullspace
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dxh = [0; 0];
%dxh = 0;
%dqh = ones(nbDOFs,1)*1;
%dqh = [-1; zeros(nbDOFs-1,1)];
for t=1:nbData
r.q(:,t) = q; %Log data
% Htmp = robot.fkine(q);
% r.x(:,t) = Htmp.t(1:2);
J = robot.jacob0(q,'trans');
J = J(1:2,:);
%J = J(2,:);
% pinvJ = pinv(J);
% pinvJ = J' / ( J * J');
% pinvJ = (J' * J) \ J';
[U,S,V] = svd(J);
S(S>0) = S(S>0).^-1;
pinvJ = V*S'*U';
%Kx = J*Kq*J'; % Stiffness at the end-effector
%robot.maniplty(q) %Manipulability index
% %Nullspace projection matrix
% N = eye(nbDOFs) - pinvJ*J;
% %N = eye(nbDOFs) - J'*pinv(J)'
% %Alternative way of computing the nullspace projection matrix, see e.g. wiki page on svd
% %(https://en.wikipedia.org/wiki/Singular-value_decomposition) or
% %http://math.stackexchange.com/questions/421813/projection-matrix-onto-null-space
% %[U,S,V] = svd(J);
sp = (sum(S,1)<1E-1); %Span of zero rows
N = V(:,sp) * V(:,sp)';
% [U,S,V] = svd(pinvJ);
% sp = (sum(S,2)<1E-1); %Span of zero rows
% N = U(:,sp) * U(:,sp)'; %N = U * [0 0 0; 0 0 0; 0 0 1] * U'
% % d = zeros(nbDOFs,1);
% % d(sp) = 1;
% % N = U * diag(d) * U';
% dqh = [5*(pi/4-q(1)); zeros(nbDOFs-1,1)];
dqh = ones(nbDOFs,1);
dq = pinvJ * dxh + N * dqh;
q = q + dq * dt;
end
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[20,10,1000,650],'color',[1 1 1]); hold on; axis off;
%plotopt = robot.plot({'noraise', 'nobase', 'noshadow', 'nowrist', 'nojaxes'});
for t=round(linspace(1,nbData,10))
colTmp = [1,1,1] - [.7,.7,.7] * t/nbData;
plotArm(r.q(:,t), ones(nbDOFs,1)*armLength, [0;0;t*0.1], .02, colTmp);
%robot.plot(r.q(:,t)', plotopt);
end
axis equal; axis tight;
%print('-dpng','graphs/demoIK01.png');
pause;
close all;
\ No newline at end of file
function demo_IK01
% Basic forward and inverse kinematics for a planar robot, with numerical Jacobian computation.
%
% If this code is useful for your research, please cite the related publication:
% @article{Silverio19TRO,
% author="Silv\'erio, J. and Calinon, S. and Rozo, L. and Caldwell, D. G.",
% title="Learning Task Priorities from Demonstrations",
% journal="{IEEE} Trans. on Robotics",
% year="2019",
% volume="35",
% number="1",
% pages="78--94",
% doi="10.1109/TRO.2018.2878355"
% }
%
% Copyright (c) 2020 Idiap Research Institute, http://idiap.ch/
% Written by Sylvain Calinon, http://calinon.ch/
%
% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
%
% PbDlib is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License version 3 as
% published by the Free Software Foundation.
%
% PbDlib is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
addpath('./m_fcts/');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dt = 1E-2; %Time step
nbData = 100;
%% Robot parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
model.nbDOFs = 2; %Number of articulations
model.l = 0.6; %Length of each link
q(:,1) = [pi/2; pi/2; pi/3]; %Initial pose
% q = rand(model.nbDOFs,1);
%% IK
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dxh = [.1; 0];
for t=1:nbData-1
J = jacob0(q(:,t), model); %Jacobian
dq = pinv(J) * dxh;
q(:,t+1) = q(:,t) + dq * dt;
x(:,t+1) = fkine(q(:,t+1), model); %FK for a planar robot
end
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[20,10,800,650],'color',[1 1 1]); hold on; axis off;
for t=floor(linspace(1,nbData,2))
colTmp = [.7,.7,.7] - [.7,.7,.7] * t/nbData;
plotArm(q(:,t), ones(model.nbDOFs,1)*model.l, [0;0;-2+t/nbData], .03, colTmp);
plot(x(1,t), x(2,t), '.','markersize',50,'color',[.8 0 0]);
end
plot(x(1,:), x(2,:), '.','markersize',20,'color',[0 0 0]);
axis equal; axis tight;
%print('-dpng','graphs/demoIK01.png');
pause;
close all;
end
%%%%%%%%%%%%%%%%%%%%%%
%Forward kinematics
function [x, Tf] = fkine(q, model)
Tf = eye(4);
T = repmat(Tf, [1,1,size(q,1)]);
for n=1:size(q,1)
c = cos(q(n));
s = sin(q(n));
T(:,:,n) = [c, -s, 0, model.l * c; ...
s, c, 0, model.l * s; ...
0, 0, 1, 0;
0, 0, 0, 1]; %Homogeneous matrix
Tf = Tf * T(:,:,n);
end
x = Tf(1:2,end);
end
%%%%%%%%%%%%%%%%%%%%%%
%Jacobian with numerical computation
function J = jacob0(q, model)
e = 1E-6;
J = zeros(2,size(q,1));
for n=1:size(q,1)
qtmp = q;
qtmp(n) = qtmp(n) + e;
J(:,n) = (fkine(qtmp, model) - fkine(q, model)) / e;
end
end
\ No newline at end of file
function demo_IK02
% Inverse kinematics with two arms and nullspace.
%
% This demo requires the robotics toolbox RTB10 (http://petercorke.com/wordpress/toolboxes/robotics-toolbox).
% First run 'startup_rvc' from the robotics toolbox.
% Inverse kinematics with nullspace projection operator.
%
% If this code is useful for your research, please cite the related publication:
% @article{Silverio19TRO,
......@@ -15,8 +12,11 @@ function demo_IK02
% pages="78--94",
% doi="10.1109/TRO.2018.2878355"
% }
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
%
% The commented parts of this demo require the robotics toolbox RTB10 (http://petercorke.com/wordpress/toolboxes/robotics-toolbox).
% First run 'startup_rvc' from the robotics toolbox if you uncomment these parts.
%
% Copyright (c) 2020 Idiap Research Institute, http://idiap.ch/
% Written by Sylvain Calinon, http://calinon.ch/
%
% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
......@@ -34,92 +34,119 @@ function demo_IK02
% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
addpath('./m_fcts/');
disp('This demo requires the robotics toolbox RTB10 (http://petercorke.com/wordpress/toolboxes/robotics-toolbox).');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dt = 0.01; %Time step
nbData = 500; %Numer of points in a trajectory
nbData = 100;
%% Create robot
%% Robot parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Robot parameters
nbDOFs = 5; %Number of articulations
armLength = 0.6; %Length of each link
L1 = Link('d', 0, 'a', armLength, 'alpha', 0);
arm = SerialLink(repmat(L1,3,1));
nbDOFs = 3; %Number of articulations
model.l = 0.6; %Length of each link
% L1 = Link('d', 0, 'a', model.l, 'alpha', 0);
% robot = SerialLink(repmat(L1,nbDOFs,1));
%Initial pose
q0(:,1) = [pi/2 pi/2 pi/3 -pi/2 -pi/3];
q = q0;
q(:,1) = [pi/2 pi/2 pi/3]; %Initial pose
%% IK with nullspace
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
r=[];
for t=1:nbData
r.q(:,t)=q; %Log data
Htmp = arm.fkine(q([1,4:5]));
r.rx(:,t) = Htmp.t(1:2);
Htmp = arm.fkine(q(1:3));
r.lx(:,t) = Htmp.t(1:2);
dxh = [0; 0];
%dxh = 0;
if t==1
rxh = [linspace(r.rx(1,1),r.rx(1,1)+.3,nbData); linspace(r.rx(2,1),r.rx(2,1)+1.2,nbData)];
lxh = [linspace(r.lx(1,1),r.lx(1,1)-.3,nbData); linspace(r.lx(2,1),r.lx(2,1)+1.2,nbData)];
end
%dqh = ones(nbDOFs,1)*1;
%dqh = [-1; zeros(nbDOFs-1,1)];
rJ = arm.jacob0(q([1,4:5]),'trans');
rJ = rJ(1:2,:);
for t=1:nbData
r.q(:,t) = q; %Log data
% Htmp = robot.fkine(q);
% r.x(:,t) = Htmp.t(1:2);
r.x(:,t) = fkine(q, model); %Log data
lJ = arm.jacob0(q(1:3),'trans');
lJ = lJ(1:2,:);
J = lJ;
J(3:4,[1,4:5]) = rJ;
% J = robot.jacob0(q,'trans');
% J = J(1:2,:);
J = jacob0(q, model);
% pinvJ = pinv(J);
% pinvJ = J' / ( J * J');
% pinvJ = (J' * J) \ J';
[U,S,V] = svd(J);
S(S>0) = S(S>0).^-1;
pinvJ = V*S'*U';
Ja = J(1:2,:);
Jb = J(3:4,:);
%Kx = J*Kq*J'; % Stiffness at the end-effector
%robot.maniplty(q) %Manipulability index
% Na = eye(nbDOFs) - pinv(Ja) * Ja; %Nullspace projection matrix
Nb = eye(nbDOFs) - pinv(Jb) * Jb; %Nullspace projection matrix
% %Nullspace projection matrix
% N = eye(nbDOFs) - pinvJ*J;
% %N = eye(nbDOFs) - J'*pinv(J)'
% %Alternative way of computing the nullspace projection matrix, see e.g. wiki page on svd
% %(https://en.wikipedia.org/wiki/Singular-value_decomposition) or
% %http://math.stackexchange.com/questions/421813/projection-matrix-onto-null-space
% %[U,S,V] = svd(J);
sp = (sum(S,1)<1E-1); %Span of zero rows
N = V(:,sp) * V(:,sp)';
ldx = 100 * (lxh(:,t) - r.lx(:,t));
rdx = 100 * (rxh(:,t) - r.rx(:,t));
% [U,S,V] = svd(pinvJ);
% sp = (sum(S,2)<1E-1); %Span of zero rows
% N = U(:,sp) * U(:,sp)'; %N = U * [0 0 0; 0 0 0; 0 0 1] * U'
% % d = zeros(nbDOFs,1);
% % d(sp) = 1;
% % N = U * diag(d) * U';
%Prioritized bimanual tracking task
Ja_mod = Ja * Nb;
pinvJa_mod = Ja_mod' / (Ja_mod * Ja_mod' + eye(2) .* 1E-1);
dqb = pinv(Jb) * rdx;
dq = dqb + Nb * pinvJa_mod * (ldx - Ja * dqb);
% dq = dqb + Nb * pinv(Ja) * ldx; %Approximation
% dqh = [5*(pi/4-q(1)); zeros(nbDOFs-1,1)];
dqh = ones(nbDOFs,1);
q = q + dq*dt;
dq = pinvJ * dxh + N * dqh;
q = q + dq * dt;
end
%% Plots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure('position',[10,10,2300,1200],'color',[1 1 1]); hold on; axis off;
listT = round(linspace(1,nbData,10));
i=0;
for t=listT
i=i+1;
colTmp = [.9,.9,.9] - [.7,.7,.7] * i/length(listT);
plotArm(r.q(1:3,t), ones(3,1)*armLength, [0;0;i], .03, colTmp); %left arm
plotArm(r.q([1,4:5],t), ones(3,1)*armLength, [0;0;i+0.1], .03, colTmp); %right arm
figure('position',[20,10,1000,650],'color',[1 1 1]); hold on; axis off;
%plotopt = robot.plot({'noraise', 'nobase', 'noshadow', 'nowrist', 'nojaxes'});
for t=round(linspace(1,nbData,10))
%robot.plot(r.q(:,t)', plotopt);
colTmp = [1,1,1] - [.7,.7,.7] * t/nbData;
plotArm(r.q(:,t), ones(nbDOFs,1)*model.l, [0;0;t*0.1], .02, colTmp);
end
plot3(rxh(1,:), rxh(2,:), ones(1,nbData)*20, '-','linewidth',2,'color',[.8 0 0]);
plot3(lxh(1,:), lxh(2,:), ones(1,nbData)*20, '-','linewidth',2,'color',[.8 0 0]);
plot3(rxh(1,listT), rxh(2,listT), ones(1,10)*20, '.','markersize',25,'color',[.8 0 0]);
plot3(lxh(1,listT), lxh(2,listT), ones(1,10)*20, '.','markersize',25,'color',[.8 0 0]);
%axis([-.2 1 -.2 1]);
axis equal; axis tight;
% print('-dpng','graphs/demoIK02.png');
%print('-dpng','graphs/demoIK02.png');
pause;
close all;
\ No newline at end of file
close all;
end
%%%%%%%%%%%%%%%%%%%%%%
%Forward kinematics
function [x, Tf] = fkine(q, model)
Tf = eye(4);
T = repmat(Tf, [1,1,size(q,1)]);
for n=1:size(q,1)
c = cos(q(n));
s = sin(q(n));
T(:,:,n) = [c, -s, 0, model.l * c; ...
s, c, 0, model.l * s; ...
0, 0, 1, 0;
0, 0, 0, 1]; %Homogeneous matrix
Tf = Tf * T(:,:,n);
end
x = Tf(1:2,end);
end
%%%%%%%%%%%%%%%%%%%%%%
%Jacobian with numerical computation
function J = jacob0(q, model)
e = 1E-6;
J = zeros(2,size(q,1));
for n=1:size(q,1)
qtmp = q;
qtmp(n) = qtmp(n) + e;
J(:,n) = (fkine(qtmp, model) - fkine(q, model)) / e;
end
end
\ No newline at end of file
function demo_IK_nullspaceAsProduct01
% 3-level nullspace control formulated as product of Gaussians (PoG).
% (see also demo_IK_weighted01.m)
%
% This demo requires the robotics toolbox RTB10 (http://petercorke.com/wordpress/toolboxes/robotics-toolbox).
% First run 'startup_rvc' from the robotics toolbox.
......
function demo_IK_quat01
% Inverse kinematics for orientation data represented as unit quaternions.
% (see also demo_IK_orient01.m)
%
% This demo requires the robotics toolbox RTB10 (http://petercorke.com/wordpress/toolboxes/robotics-toolbox).
% First run 'startup_rvc' from the robotics toolbox.
% Copyright (c) 2020 Idiap Research Institute, http://idiap.ch/
% Written by Sylvain Calinon, http://calinon.ch/
%
% Copyright (c) 2019 Idiap Research Institute, http://idiap.ch/
% Written by João Silvério and Sylvain Calinon
% The commented parts of this demo require the robotics toolbox RTB10 (http://petercorke.com/wordpress/toolboxes/robotics-toolbox).
% First run 'startup_rvc' from the robotics toolbox if you uncomment these parts.
%
% This file is part of PbDlib, http://www.idiap.ch/software/pbdlib/
%
......@@ -22,34 +23,28 @@ function demo_IK_quat01
% along with PbDlib. If not, see <http://www.gnu.org/licenses/>.
addpath('./m_fcts/');
disp('This demo requires the robotics toolbox RTB10 (http://petercorke.com/wordpress/toolboxes/robotics-toolbox).');
%% Parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dt = 0.01; %Time step
nbData = 100;
nbData = 100; %Number of points in a trajectory
Kp = 1E-1; %Tracking gain