text:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = INVERSEKINEMATIC_KUKA_KR_16_ARC_HW(robot, T)
%   Solves the inverse kinematic problem for the KUKA KR5 ARC robot
%   where:
%   robot stores the robot parameters.
%   T is an homogeneous transform that specifies the position/orientation
%   of the end effector.
%
%   A call to Q=INVERSEKINEMATIC_KUKA_KR_16_ARC_HW returns 8 possible solutions, thus,
%   Q is a 6x8 matrix where each column stores 6 feasible joint values.
%
%
%   Example code:
%
%   robot=load_robot('kuka', 'KUKA_KR_16_ARC_HW');
%   q = [0 0 0 0 0 0];
%   T = directkinematic(robot, q);
%   %Call the inversekinematic for this robot
%   qinv = inversekinematic(robot, T);
%   check that all of them are feasible solutions!
%   and every Ti equals T
%   for i=1:8,
%        Ti = directkinematic(robot, qinv(:,i))
%   end
%	See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function q = inversekinematic_kuka_kr_16_arc_HW(robot, T)

%initialize q,
%eight possible solutions are generally feasible
q=zeros(6,8);

% %Evaluate the parameters
% theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
L6=abs(d(6));

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Compute the position of the wrist, being W the Z component of the end effector's system
W = T(1:3,3);

% Pm: wrist position
Pm = [Px Py Pz]' - L6*W;

%first joint, two possible solutions admited:
% if q(1) is a solution, then q(1) + pi is also a solution
q1=atan2(Pm(2), Pm(1));

%solve for q2
q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);
%the other possible solution is q1 + pi
q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);

%solve for q3
q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);
%solver for q3 for both cases
q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);

%the next matrix doubles each column. For each two columns, two different
%configurations for theta4, theta5 and theta6 will be computed. These
%configurations are generally referred as wrist up and wrist down solution
q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;
     q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
     q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0];

%leave only the real part of the solutions
q=real(q);

%Note that in this robot, the joint q3 has a non-simmetrical range. In this
%case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing
%step is avoided in this angle (the next line is commented). When solving
%for the orientation, the solutions are normalized to the [-pi, pi] range
%only for the theta4, theta5 and theta6 joints.

%normalize q to [-pi, pi]
q(1,:) = normalize(q(1,:));
q(2,:) = normalize(q(2,:));
% solve for the last three joints
% for any of the possible combinations (theta1, theta2, theta3)
for i=1:2:size(q,2),
    qtemp = solve_spherical_wrist(robot, q(:,i), T, 1,'geometric'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i)=qtemp;

    qtemp = solve_spherical_wrist(robot, q(:,i), T, -1, 'geometric'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i+1)=qtemp;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for second joint theta2, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q2 = solve_for_theta2(robot, q, Pm)

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%See geometry
L2=abs(a(2));
L3=abs(d(4));
A2 = abs(a(3));

%See geometry of the robot
%compute L4
L4 = sqrt(A2^2 + L3^2);

%The inverse kinematic problem can be solved as in the IRB 140 (for example)

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = atan2(-p1(2), p1(1));
gamma = real(acos((L2^2+r^2-L4^2)/(2*r*L2)));

%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta3
q2(1) = pi/2 - beta - gamma; %elbow up
q2(2) = pi/2 - beta + gamma; %elbow down

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for third joint theta3, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q3 = solve_for_theta3(robot, q, Pm)

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%See geometry
L2=abs(a(2));
L3=abs(d(4));

A2 = abs(a(3));

%See geometry of the robot
%compute L4
L4 = sqrt(A2^2 + L3^2);

%the angle phi is fixed
phi=acos((A2^2+L4^2-L3^2)/(2*A2*L4));

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = real(acos((L2^2 + L4^2 - r^2)/(2*L2*L4)));

%return two possible solutions
%elbow up and elbow down solutions
%the order here is important
q3(1) = pi - phi - beta;
q3(2) = pi - phi + beta;

repo: ARTE


text:
% TEST_CUBE_G_NMNN: data function for Neumann boundary condition.

function g = test_cube_g_nmnn (x, y, z, ind)
  switch ind
    case 1
      g = -exp (x + z) .* sin (y);
    case 2
      g = exp (x + z) .* sin (y);
    case 3
      g = -exp (x + z) .* cos (y);
    case 4
      g = exp (x + z) .* cos (y);
    case 5
      g = -exp (x + z) .* sin (y);
    case 6
      g = exp (x + z) .* sin (y);
    otherwise
      error ('test_cube_g_nmnn: unknown reference number');
  end
end

author: rafavzqz

repo: geopdes

save_path: github-repos/MATLAB/rafavzqz-geopdes


text:
function atlasStore(vw, images, whichAtlas, curSlice)
% Store angle and ecc atlas data into an atlas datatype in a flat view
% (incorporated from "atlasCreate.m").
%
% atlasStore(vw, images, whichAtlas, curSlice)
%
% Example
%   vw = FLAT{1};
%   atlasStore(vw,images,[],[]);
%
% Author: KA, BW

global dataTYPES

% Check arguments
if notDefined('vw'),            vw = getCurView; end
if notDefined('images'),        error('images must be defined'); end
if notDefined('whichAtlas'),    whichAtlas = atlasSelectAtlas; end
if notDefined('curSlice'),      curSlice = viewGet(vw, 'Current Slice'); end

sz = viewGet(vw,'anatSize');
atlasAngle = imresize(images.dA2,sz(1)/size(images.dA2,1),'Method','nearest');
atlasEcc = imresize(images.dA1,sz(1)/size(images.dA1,1),'Method','nearest');
areasImg = imresize(double(images.dareasImg),sz(1)/size(images.dareasImg,1),'Method','nearest');

atlasTypeList = {'angle', 'eccentricity'};
nAtlases = length(atlasTypeList);
atlasDim = dataSize(vw,1);

% If needed, create a new atlas.  Otherwise take the existing atlas
if whichAtlas == 0   % Create a new Atlas
    % Build the atlas sub-directory and data type.
    [atlasName,whichAtlas] = addAtlas2DataTYPE;
else
    atlasName = dataTYPES(whichAtlas).name;
end

% update dataTYPES
% Copy the params from the first scan of the current data type (usually
% Averages or some other readl data) into the dataTYPE we will use for the
% new atlas.
wedgePhaseShift=0;
ringPhaseShift = 0;
retPhases = [0 pi*2 pi/2 pi];

dtCopy(vw.curDataType,whichAtlas,atlasTypeList,curSlice,...
    [wedgePhaseShift,ringPhaseShift],retPhases);

% Init the atlasView with the data in the currently selected atlas view.
atlasView = atlasInit(vw,atlasName,nAtlases,whichAtlas,atlasDim);

% These are the atlas data.  There is a problem with the atlasAngle at one
% edge.  Look into why.
atlasView.ph{1}(:,:,curSlice) = atlasAngle;
atlasView.ph{2}(:,:,curSlice) = atlasEcc;

% This is the shape which gets deformed as the atlas fitting proceeds.  The
% edges are used as an overlay on the data so you can see the boundaries.
atlasView.co{1}(:,:,curSlice) = round(areasImg);
atlasView.co{2}(:,:,curSlice) = round(areasImg);

atlasView.amp{1}(:,:,curSlice) = zeros(size(atlasAngle));
atlasView.amp{2}(:,:,curSlice) = zeros(size(atlasEcc));

% Save files and update information in the atlas view.
saveSession;
saveCorAnal(atlasView,[],[],[],1);

return;

author: vistalab

repo: vistasoft

save_path: github-repos/MATLAB/vistalab-vistasoft