├── .idea ├── .gitignore ├── encodings.xml ├── misc.xml ├── vcs.xml ├── modules.xml ├── inspectionProfiles │ └── Project_Default.xml ├── IK_Solver.iml ├── markdown-navigator-enh.xml └── markdown-navigator.xml ├── MATLAB ├── rnum.mat ├── getimage.m ├── plot_xy.m ├── isOctave.m ├── plot_xyz.m ├── putimage.m ├── ik_jtm.m ├── ik_pim2.m ├── transform.m ├── rotation.m ├── get_endeff_rot.m ├── clamp_rot.m ├── ik_dls.m ├── image2avi.m ├── closing_gain.m ├── jacobian.m ├── ik_pim3.m ├── get_endeff_ypr.m ├── angle_chk.m ├── time_to_goal.m ├── avelE_wrt_jointm.m ├── avelT_wrt_jointm.m ├── solve_chk.m ├── null_space_control.m └── IK_Solver.m ├── refs ├── iksurvey.pdf ├── content_2012_4_10_60013.pdf └── Practical 01 - Inverse Kinematics.pdf ├── docs └── images │ ├── Hollerith_card.jpg │ ├── IK_Solver_3_2D.mp4 │ ├── IK_Solver_3_3D.mp4 │ ├── IK_Solver_3_2D_001.jpg │ ├── IK_Solver_3_2D_140.jpg │ ├── IK_Solver_3_3D_001.jpg │ ├── IK_Solver_3_3D_140.jpg │ ├── Blender_IK_Solver_7_3D_00.png │ ├── Blender_IK_Solver_7_3D_97.png │ ├── Blender_IK_Solver_7_GL_00.png │ └── Blender_IK_Solver_7_GL_97.png ├── Blender ├── FourLink3D │ ├── fourlink3d.blend │ ├── renders │ │ ├── imgCGI_anim.gif │ │ └── imgOGL_anim.gif │ ├── scripts │ │ ├── addons │ │ │ └── ArmatureRot.py │ │ └── IK_SolverPanel.py │ └── README.txt ├── FourLink3D_BGE │ ├── fourlink3d_BGE.blend │ └── scripts │ │ ├── timeText.py │ │ └── moveEmpty.py └── README.txt ├── .gitignore ├── util ├── README.txt ├── pngtogif.sh ├── pngs2agif.py └── gifmaker.py ├── DISCLAIMER ├── IK_Solver_nlink.py ├── LICENSE-LGPLv3 ├── IK_Solver_server.py ├── README.md ├── IK_Solver_funcs.py ├── IK_Solver.py ├── IK_Solver_class.py └── IK_Solver_client.py /.idea/.gitignore: -------------------------------------------------------------------------------- 1 | # Default ignored files 2 | /workspace.xml -------------------------------------------------------------------------------- /MATLAB/rnum.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gedeschaines/IK_Solver/HEAD/MATLAB/rnum.mat -------------------------------------------------------------------------------- /refs/iksurvey.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gedeschaines/IK_Solver/HEAD/refs/iksurvey.pdf -------------------------------------------------------------------------------- /docs/images/Hollerith_card.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gedeschaines/IK_Solver/HEAD/docs/images/Hollerith_card.jpg -------------------------------------------------------------------------------- /docs/images/IK_Solver_3_2D.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gedeschaines/IK_Solver/HEAD/docs/images/IK_Solver_3_2D.mp4 -------------------------------------------------------------------------------- /docs/images/IK_Solver_3_3D.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gedeschaines/IK_Solver/HEAD/docs/images/IK_Solver_3_3D.mp4 -------------------------------------------------------------------------------- /refs/content_2012_4_10_60013.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gedeschaines/IK_Solver/HEAD/refs/content_2012_4_10_60013.pdf -------------------------------------------------------------------------------- /Blender/FourLink3D/fourlink3d.blend: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gedeschaines/IK_Solver/HEAD/Blender/FourLink3D/fourlink3d.blend -------------------------------------------------------------------------------- /docs/images/IK_Solver_3_2D_001.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gedeschaines/IK_Solver/HEAD/docs/images/IK_Solver_3_2D_001.jpg -------------------------------------------------------------------------------- /docs/images/IK_Solver_3_2D_140.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gedeschaines/IK_Solver/HEAD/docs/images/IK_Solver_3_2D_140.jpg -------------------------------------------------------------------------------- /docs/images/IK_Solver_3_3D_001.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gedeschaines/IK_Solver/HEAD/docs/images/IK_Solver_3_3D_001.jpg -------------------------------------------------------------------------------- /docs/images/IK_Solver_3_3D_140.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gedeschaines/IK_Solver/HEAD/docs/images/IK_Solver_3_3D_140.jpg -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .idea/* 2 | MATLAB/octave-workspace 3 | MATLAB/images/* 4 | MATLAB/images/jpgs/* 5 | temp/* 6 | __pycache__/* 7 | *.py[co] 8 | *~ -------------------------------------------------------------------------------- /Blender/FourLink3D/renders/imgCGI_anim.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gedeschaines/IK_Solver/HEAD/Blender/FourLink3D/renders/imgCGI_anim.gif -------------------------------------------------------------------------------- /Blender/FourLink3D/renders/imgOGL_anim.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gedeschaines/IK_Solver/HEAD/Blender/FourLink3D/renders/imgOGL_anim.gif -------------------------------------------------------------------------------- /docs/images/Blender_IK_Solver_7_3D_00.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gedeschaines/IK_Solver/HEAD/docs/images/Blender_IK_Solver_7_3D_00.png -------------------------------------------------------------------------------- /docs/images/Blender_IK_Solver_7_3D_97.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gedeschaines/IK_Solver/HEAD/docs/images/Blender_IK_Solver_7_3D_97.png -------------------------------------------------------------------------------- /docs/images/Blender_IK_Solver_7_GL_00.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gedeschaines/IK_Solver/HEAD/docs/images/Blender_IK_Solver_7_GL_00.png -------------------------------------------------------------------------------- /docs/images/Blender_IK_Solver_7_GL_97.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gedeschaines/IK_Solver/HEAD/docs/images/Blender_IK_Solver_7_GL_97.png -------------------------------------------------------------------------------- /refs/Practical 01 - Inverse Kinematics.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gedeschaines/IK_Solver/HEAD/refs/Practical 01 - Inverse Kinematics.pdf -------------------------------------------------------------------------------- /Blender/FourLink3D_BGE/fourlink3d_BGE.blend: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gedeschaines/IK_Solver/HEAD/Blender/FourLink3D_BGE/fourlink3d_BGE.blend -------------------------------------------------------------------------------- /.idea/encodings.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /MATLAB/getimage.m: -------------------------------------------------------------------------------- 1 | function [ img ] = getimage (fname) 2 | %% Reads and returns image from named file. 3 | %% 4 | %% see putimage 5 | 6 | [img, map, alpha] = imread(fname); 7 | 8 | endfunction -------------------------------------------------------------------------------- /.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /.idea/vcs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /MATLAB/plot_xy.m: -------------------------------------------------------------------------------- 1 | function [X,Y] = plot_xy(n,p) 2 | 3 | % function [X,Y] = plot_xy(n,p) 4 | % p = set of n link joint position vectors 5 | 6 | X = zeros(n,1); 7 | Y = zeros(n,1); 8 | for i = 1:n 9 | X(i) = p{i}(1); 10 | Y(i) = p{i}(2); 11 | end 12 | 13 | end 14 | -------------------------------------------------------------------------------- /MATLAB/isOctave.m: -------------------------------------------------------------------------------- 1 | %% 2 | %% Return: true if the environment is Octave. 3 | %% 4 | function retval = isOctave 5 | persistent cacheval; % speeds up repeated calls 6 | if isempty (cacheval) 7 | cacheval = (exist ("OCTAVE_VERSION", "builtin") > 0); 8 | end 9 | retval = cacheval; 10 | end -------------------------------------------------------------------------------- /.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /MATLAB/plot_xyz.m: -------------------------------------------------------------------------------- 1 | function [X,Y,Z] = plot_xyz(n,p) 2 | 3 | % function [X,Y,Z] = plot_xyz(n,p) 4 | % p = set of n link joint position vectors 5 | 6 | X = zeros(n,1); 7 | Y = zeros(n,1); 8 | Z = zeros(n,1); 9 | for i = 1:n 10 | X(i) = p{i}(1); 11 | Y(i) = p{i}(2); 12 | Z(i) = p{i}(3); 13 | end 14 | 15 | end 16 | -------------------------------------------------------------------------------- /MATLAB/putimage.m: -------------------------------------------------------------------------------- 1 | function [ fpath ] = putimage(h, i, fmt, iW, iH) 2 | %% Saves image associated with given figure handle 'h'and frame index 3 | %% 'i' in format specified by 'fmt' and size by iW x iH. Returns path 4 | %% to saved image file. 5 | 6 | fpath = sprintf ("./images/%ss/img_%04d.%s", fmt, i, fmt); 7 | psiz = sprintf("-S%d,%d", iW, iH); 8 | print(h, fpath, psiz); 9 | 10 | endfunction 11 | -------------------------------------------------------------------------------- /.idea/inspectionProfiles/Project_Default.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 10 | -------------------------------------------------------------------------------- /MATLAB/ik_jtm.m: -------------------------------------------------------------------------------- 1 | function [dq] = ik_jtm(J,de,dqlim) 2 | 3 | % function [dq] = ik_jtm(J,de,dqlim) : solves de = J*dq for dq using 4 | % the Jacobian transpose method 5 | % J = Jacobian matrix of de/dq 6 | % de = vector of delta errors 7 | % dqlim = vector of joint rotation delta limits 8 | 9 | dq = J'*de'; 10 | jdq = J*dq; 11 | alpha = (de*jdq)/(norm(jdq')^2); 12 | dqmax = max(abs(dq)); 13 | beta = dqlim(1)/dqmax; 14 | dq = min([alpha,beta])*dq'; 15 | 16 | end 17 | -------------------------------------------------------------------------------- /util/README.txt: -------------------------------------------------------------------------------- 1 | Animated GIF creation utility scripts. 2 | 3 | pngtogif.sh - Bash shell script to convert a sequence of PNG files to GIF files 4 | and merge the GIF files into an animated GIF using ImageMagick 5 | convert program. 6 | 7 | pngs2agif.py - Python program which converts series of PNG "####.png" files to 8 | a single animated GIF "frames_0000_####.gif" file using the Python 9 | Imaging Library (PIL) and the gifmaker.py script obtained from the 10 | source distribution of PIL. 11 | -------------------------------------------------------------------------------- /MATLAB/ik_pim2.m: -------------------------------------------------------------------------------- 1 | function [dq] = ik_pim2(J,de,sdel,dqlim) 2 | 3 | % function [dq] = ik_pim2(J,de,sdel,dqlim) : solves de = J*dq for dq using 4 | % the pseudo-inverse method from 5 | % reference [2] 6 | % J = Jacobian matrix of de/dq 7 | % de = vector of delta errors 8 | % sdel = singularity prevention damping constant 9 | % dtlim = vector of joint rotation delta limits 10 | 11 | n = size(J,2); 12 | A = J.'*J + sdel*eye(n); 13 | b = J.'*de.'; 14 | dq = (A \ b).'; 15 | dqmax = max(abs(dq)); 16 | if dqmax > dqlim(1) 17 | dq = (dqlim(1)/dqmax)*dq; 18 | end 19 | 20 | end 21 | -------------------------------------------------------------------------------- /Blender/FourLink3D_BGE/scripts/timeText.py: -------------------------------------------------------------------------------- 1 | import bpy 2 | 3 | # Run this script once to enable animation of frame time 4 | # text in the 3D viewport while in Blender Render mode. 5 | # 6 | # Derived from code presented at following web page: 7 | # 8 | # https://blender.stackexchange.com/questions/7904/how-can-i-make-dynamic-text-in-an-animation 9 | 10 | ### Time text display handler 11 | 12 | scene = bpy.context.scene 13 | ttobj = scene.objects['Text_Time'] 14 | 15 | def time_text(scene): 16 | fps = scene.render.fps / scene.render.fps_base # actual framerate 17 | ttobj.data.body = 'Time: {0: 6.3f} secs'.format(scene.frame_current/fps) 18 | 19 | bpy.app.handlers.frame_change_pre.append(time_text) -------------------------------------------------------------------------------- /MATLAB/transform.m: -------------------------------------------------------------------------------- 1 | function [p,w] = transform(n,r,u,a) 2 | 3 | % function [p,w] = transform(n,r,u,a) : transforms a and u to p and w 4 | % r = vector of n-1 joint rotations (radians) 5 | % u = set of n-1 joint rotation axis unit vectors 6 | % a = set of n link joint attachment location vectors to transform 7 | 8 | p = a; 9 | for i = 1:1:n 10 | p{i} = a{i}; 11 | j = i - 1; 12 | while j > 0 13 | p{i} = (rotation(r(j),u{j})*p{i}.').'; 14 | p{i} = p{i} + a{j}; 15 | j = j - 1; 16 | end 17 | end 18 | 19 | w = u; 20 | for i = 1:1:n-1 21 | j = i; 22 | while j > 0 23 | w{i} = (rotation(r(j),u{j})*w{i}.').'; 24 | j = j - 1; 25 | end 26 | w{i} = w{i}/norm(w{i}); 27 | end 28 | -------------------------------------------------------------------------------- /MATLAB/rotation.m: -------------------------------------------------------------------------------- 1 | function [R] = rotation(r,u) 2 | 3 | % function [R] = rotation(r,u) : returns local to global rotation matrix 4 | % r = joint rotation (radians) 5 | % u = joint rotation axis unit vector 6 | 7 | theta = r/2; 8 | costh = cos(theta); 9 | sinth = sin(theta); 10 | 11 | q0 = costh; 12 | q1 = sinth*u(1); 13 | q2 = sinth*u(2); 14 | q3 = sinth*u(3); 15 | 16 | q00 = q0*q0; 17 | q01 = q0*q1; 18 | q02 = q0*q2; 19 | q03 = q0*q3; 20 | q11 = q1*q1; 21 | q12 = q1*q2; 22 | q13 = q1*q3; 23 | q22 = q2*q2; 24 | q23 = q2*q3; 25 | q33 = q3*q3; 26 | 27 | R = [q00+q11-q22-q33, 2*(q12-q03), 2*(q13+q02); 28 | 2*(q12+q03), q00-q11+q22-q33, 2*(q23-q01); 29 | 2*(q13-q02), 2*(q23+q01), q00-q11-q22+q33]; 30 | 31 | end 32 | -------------------------------------------------------------------------------- /.idea/IK_Solver.iml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 16 | -------------------------------------------------------------------------------- /MATLAB/get_endeff_rot.m: -------------------------------------------------------------------------------- 1 | function [phiZ] = get_endeff_rot(nq,q,u) 2 | 3 | % function [phiZ] = get_endeff_rot(nq,q,u) : get current end effector rotation angle 4 | % in radians for rotation about the last 5 | % link segment z-axis. 6 | % 7 | % nq = number of joints 8 | % q = vector of joint rotations (rads) 9 | % u = set of joint rotation axis unit vectors 10 | 11 | % Rotation matrix from world space to last joint local space 12 | R = eye(3,3); 13 | for i = 1 : nq 14 | Ri = transpose(rotation(q(i),u{i})); 15 | R = Ri*R; 16 | end 17 | % World space X-axis in last joint local space 18 | vecX = transpose(R*transpose([1.0,0.0,0.0])); 19 | % Angle between last link segment and X-axis 20 | phiZ = atan2(vecX(2),vecX(1)); 21 | 22 | end 23 | -------------------------------------------------------------------------------- /MATLAB/clamp_rot.m: -------------------------------------------------------------------------------- 1 | function [r] = clamp_rot(n,r,rmin,rmax) 2 | 3 | % function [r] = clamp_rot(n,r,rmin,rmax) : returns r within [rmin,rmax] 4 | % r = vector of n joint rotations (radians) 5 | % rmin = vector of n joint rotation minimums (radians) 6 | % rmax = vector of n joint rotation maximums (radians) 7 | 8 | global p360rad; 9 | global n360rad; 10 | global p180rad; 11 | global n180rad; 12 | 13 | %global dpr; 14 | %display(sprintf('clamp_rot: r= %8.2f, %8.2f %8.2f', ... 15 | % r(1)*dpr, r(2)*dpr, r(3)*dpr)); 16 | 17 | for i = 1:n 18 | if abs(rmax(i)-rmin(i)) >= p360rad 19 | if r(i) < n180rad 20 | r(i) = p360rad + r(i); 21 | end 22 | if r(i) > p180rad 23 | r(i) = r(i) + n360rad; 24 | end 25 | end 26 | if r(i) < rmin(i) 27 | r(i) = rmin(i); 28 | elseif r(i) > rmax(i) 29 | r(i) = rmax(i); 30 | end 31 | end 32 | 33 | end 34 | -------------------------------------------------------------------------------- /MATLAB/ik_dls.m: -------------------------------------------------------------------------------- 1 | function [dq] = ik_dls(J,de,slam,dqlim,phi) 2 | 3 | % function [qq] = ik_dls(J,de,slam,dqlim,phi) : solves de = J*dq for dq using 4 | % damped least squares with SVD 5 | % from reference [3]. 6 | % J = Jacobian matrix of de/dq 7 | % de = vector of delta errors 8 | % slam = singularity damping factor 9 | % dqlim = vector of joint rotation delta limits 10 | % phi = null space control vector 11 | 12 | [m,n] = size(J); 13 | [U,S,V] = svd(J); 14 | Sinv = zeros(n,m); 15 | for i = 1:m 16 | Sinv(i,i) = S(i,i)/(S(i,i)^2 + slam^2); 17 | end 18 | Jinv = V*(Sinv*U.'); 19 | Jprj = eye(n) - Jinv*J; 20 | dq = (Jinv*de.' + Jprj*phi.').'; 21 | dqmax = max(abs(dq)); 22 | for i = 1:length(dq) 23 | if dqmax > dqlim(i) 24 | dq(i) = dq(i)*(dqlim(i)/dqmax); 25 | end 26 | end 27 | 28 | end 29 | -------------------------------------------------------------------------------- /MATLAB/image2avi.m: -------------------------------------------------------------------------------- 1 | function [status] = image2avi (fmt, fps, w, h, name) 2 | %% Utilizes 'avconv' libav-tool to convert sequence of images 3 | %% with given format type 'fmt' to named AVI movie file with 4 | %% frame rate 'fps' and frame size 'w' x 'h'. Returns avconv 5 | %% exit status. 6 | %% 7 | %% see putimage 8 | 9 | if exist("/usr/bin/avconv", 'file') 10 | ffmt = "-y -f image2 "; 11 | rate = sprintf ("-r %d ", fps); 12 | imgs = sprintf ("-i ./images/%ss/img_%s.%s ", fmt, '%04d', fmt); 13 | cdec = "-c mjpeg "; 14 | imsz = sprintf ("-s %dx%d ", w, h); 15 | rdir = " 2>&1"; 16 | scmd = ["/usr/bin/avconv ", ffmt, rate, imgs, cdec, rate, imsz, name, rdir]; 17 | 18 | [status, output] = system (scmd, true, "sync"); 19 | 20 | if status ~= 0 21 | printf ("%s", output); 22 | end 23 | 24 | else 25 | printf ("requires avconv libav-tool to create movie.\n"); 26 | end 27 | 28 | endfunction 29 | -------------------------------------------------------------------------------- /util/pngtogif.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | #FILE: pngtogif.sh 4 | #DATE: 05 AUG 2012 5 | #AUTH: G. E. Deschaines 6 | #DESC: Converts a sequence of PNG files to GIF files and merges 7 | # the GIF files into an animated GIF file. 8 | 9 | #NOTE: Requires the ImageMagick convert program. 10 | 11 | # Get list of PNG files. 12 | 13 | PNG_LIST=(`ls *.png`) 14 | if [ "${#PNG_LIST[*]}" == "0" ] 15 | then 16 | echo "error: No PNG files in present working directory." 17 | exit -1 18 | fi 19 | 20 | # Convert each PNG file into a GIF file. 21 | 22 | GIF_LIST="" 23 | for FILE in ${PNG_LIST[@]} 24 | do 25 | NAME=${FILE%.png} 26 | echo "converting: $FILE" 27 | convert $FILE $NAME.gif 28 | GIF_LIST="$GIF_LIST $NAME.gif" 29 | done 30 | 31 | # Merge all GIF files into the animated gif file. 32 | 33 | echo "Creating animated gif file: img_anim.gif" 34 | convert -dispose None -delay 8 $GIF_LIST -loop 0 img_anim.gif 35 | 36 | exit 0 37 | 38 | -------------------------------------------------------------------------------- /MATLAB/closing_gain.m: -------------------------------------------------------------------------------- 1 | function [gain] = closing_gain(ikm,ec,pt,vt) 2 | 3 | % function [gain] = closing_gain(ikm,ec,pt,vt) : returns end effector 4 | % closing gain 5 | % ikm = IKmethod 6 | % ec = end effector current position in world space 7 | % pt = position of target in world space 8 | % vt = velocity of target in world space 9 | 10 | de = ec - pt; 11 | nde = norm(de); 12 | nvt = norm(vt); 13 | if (nde == 0.0) || (nvt == 0.0) 14 | gain = 1.0; 15 | else 16 | % compute cosine of approach angle relative to target velocity vector 17 | aacos = dot(de/nde,vt/nvt); 18 | if aacos <= 0.0 19 | gain = 1.6; 20 | else 21 | if aacos <= 0.7071 22 | gain = 1.2; 23 | else 24 | if ikm < 6 25 | gain = 1.0; % without null space control 26 | else 27 | gain = 0.8; % with null space control 28 | end 29 | end 30 | end 31 | end 32 | 33 | end 34 | -------------------------------------------------------------------------------- /MATLAB/jacobian.m: -------------------------------------------------------------------------------- 1 | function [J] = jacobian(n,r,p,e) 2 | 3 | % function [J] = jacobian(n,r,p,e) : returns de/dq Jacobian matrix for 4 | % e representing a 1x3 vector in 5 | % world space [x,y,z] coordinates 6 | % r = set of n joint rotation direction vectors (i.e., a set 7 | % {r1, r2, r3, ... rn} where r's are vectors of the form 8 | % [rx,ry,rz]). 9 | % p = set of at least n-1 link joint position vectors (i.e., 10 | % {p1, p2, p3, ...} where p's are vectors of the form 11 | % [px,py,pz]). 12 | % e = desired effector position vector (i.e., [ex,ey,ez]). 13 | 14 | J = zeros(3,n); 15 | for i = 1:n 16 | j = cross(r{i},(e - p{i})); % de/dq = r x (e - p) 17 | J(1,i) = j(1); % | dex/dq1 dex/dq2 dex/dq3 ... dex/dqn | 18 | J(2,i) = j(2); % | dey/dq1 dey/dq2 dey/dq3 ... dey dqn | 19 | J(3,i) = j(3); % | dez/dq1 dez/dq2 dez/dq3 ... dez/dqn | 20 | end 21 | 22 | end 23 | -------------------------------------------------------------------------------- /MATLAB/ik_pim3.m: -------------------------------------------------------------------------------- 1 | function [dq] = ik_pim3(J,de,sfac,dqlim,phi) 2 | 3 | % function [dq] = ik_pim3(J,de,sfac,dqlim,phi) : solves de = J*dq for dq using 4 | % the pseudo-inverse method from 5 | % reference [3]. 6 | % J = Jacobian matrix of de/dq 7 | % de = vector of delta errors 8 | % sfac = singularity threshold factor 9 | % dqlim = vector of joint rotation delta limits 10 | % phi = null space control vector 11 | 12 | [m,n] = size(J); 13 | [U,S,V] = svd(J); 14 | 15 | slim = sfac*max(abs(diag(S))); 16 | Sinv = zeros(n,m); 17 | for i = 1:m 18 | if abs(S(i,i)) > slim; 19 | Sinv(i,i) = 1.0/S(i,i); 20 | end 21 | end 22 | 23 | Jinv = V*(Sinv*U.'); 24 | Jprj = eye(n) - Jinv*J; 25 | dq = (Jinv*de.' + Jprj*phi.').'; 26 | dqmax = max(abs(dq)); 27 | for i = 1:length(dq) 28 | if dqmax > dqlim(i) 29 | dq(i) = dq(i)*(dqlim(i)/dqmax); 30 | end 31 | end 32 | 33 | end 34 | -------------------------------------------------------------------------------- /MATLAB/get_endeff_ypr.m: -------------------------------------------------------------------------------- 1 | function [yaw,pitch,roll] = get_endeff_ypr(nq,q,u) 2 | 3 | % function [yaw,pich,roll] = get_endeff_ypr(nq,q,u) : get current end effector yaw, 4 | % pitch and roll angles in radians 5 | % for Euler rotations about the last 6 | % link segment zyx axes respectively. 7 | % Assumes the pitch angle never 8 | % reaches +/- 90 degrees. 9 | % 10 | % nq = number of joints 11 | % q = vector of joint rotations (rads) 12 | % u = set of joint rotation axis unit vectors 13 | 14 | % Rotation matrix from world space to last joint local space 15 | R = eye(3,3); 16 | for i = 1 : nq 17 | Ri = transpose(rotation(q(i),u{i})); 18 | R = Ri*R; 19 | end 20 | % Euler rotation angles of last link segment 21 | yaw = atan2(R(1,2),R(1,1)); 22 | pitch = -asin(R(1,3)); 23 | roll = atan2(R(2,3),R(3,3)); 24 | 25 | end 26 | -------------------------------------------------------------------------------- /MATLAB/angle_chk.m: -------------------------------------------------------------------------------- 1 | function angle_chk(q,u,p,et) 2 | 3 | % function angle_chk(q,u,p,et) 4 | % q = vector of n joint rotations (radians) 5 | % u = set of n joint local rotation axis unit vectors 6 | % p = set of n link joint position vectors 7 | % et = end effector target position vector 8 | 9 | global dpr; 10 | 11 | display('* Angle Check:'); 12 | n = length(q); 13 | k = 1; 14 | a = q(k)*dpr; 15 | fprintf('joint %1d rotation angle from inertial x-axis = %12.6f\n',k,a); 16 | for k = 2:n 17 | a = q(k)*dpr; 18 | fprintf('joint %1d rotation angle from joint %1d x-axis = %12.6f\n',k,k-1,a); 19 | end 20 | asum = sum(q)*dpr; 21 | fprintf('angle to joint %1d x-axis from inertial x-axis = %12.6f\n',n,asum); 22 | etp = et - p{n}; % vector from joint n to target 23 | det = norm(etp); % distance from joint n to target 24 | at3 = acos((etp*(rotation(sum(q),u{n})*[1;0;0]))/det)*dpr; 25 | atI = acos((etp*[1;0;0])/det)*dpr; 26 | fprintf('angle to target from joint %1d x-axis = %12.6f\n',n,at3); 27 | fprintf('angle to target from inertial x-axis = %12.6f\n',atI); 28 | 29 | end 30 | -------------------------------------------------------------------------------- /MATLAB/time_to_goal.m: -------------------------------------------------------------------------------- 1 | function [tgo] = time_to_goal(pt,vt,w,p,dq) 2 | 3 | % function [tgo] = time_to_goal(pt,vt,w,p,dq) : returns estimated time for 4 | % end-effector to reach the 5 | % target goal 6 | % pt = current position of target in world space 7 | % vt = current velocity of target in world space 8 | % w = set of n joint rotation direction vectors in world space 9 | % p = set of n+1 position vectors (n joints + end-effector) in world space 10 | % dq = vector of n joint rotation rates (radians/sec) 11 | 12 | n = length(dq); 13 | ie = length(p); 14 | % absolute velocity of end effector 15 | ve = zeros(1,3); 16 | for i = n:-1:1 17 | ve = ve + cross(dq(i)*w{i},p{i+1}-p{i}); 18 | end 19 | % closing vector between end-effector and target 20 | vecp = pt - p{ie}; 21 | nrmp = norm(vecp); 22 | % estimated time to go 23 | if nrmp > 0.0 24 | vecn = vecp/nrmp; 25 | tgo = nrmp/(dot(ve-vt,vecn)); 26 | if tgo < 0.0 27 | tgo = 0.0; 28 | end 29 | else 30 | tgo = 0.0; 31 | end 32 | 33 | end 34 | -------------------------------------------------------------------------------- /util/pngs2agif.py: -------------------------------------------------------------------------------- 1 | #!D:\Progs\Python25\python 2 | """ FILE: pngs2agif.py 3 | DATE: 29 OCT 2010 4 | AUTH: Gary E. Deschaines (gedesch@hotmail.com) 5 | 6 | Converts series of png "####.png" files to a single 7 | animated GIF "frames_0000_####.gif" file using the Python 8 | Imaging Library (PIL) and the gifmaker.py script obtained 9 | from the source distribution of PIL. 10 | """ 11 | 12 | from PIL import Image 13 | from locale import format_string 14 | 15 | import glob 16 | import gifmaker 17 | 18 | if __name__ == "__main__": 19 | 20 | seq = [] # sequence of converted images 21 | num = -1 # counter of converted images minus 1 22 | idir = "./pngs/" # input file directory 23 | odir = "./gifs/" # output file directory 24 | 25 | # Assemble sequence of converted images 26 | for ifile in glob.glob(idir + "*.png"): 27 | im = Image.open(ifile, "r") 28 | imc = im.convert('P') 29 | seq.append(imc) 30 | print "Loaded %s and converted from %s to %s" % \ 31 | (ifile, im.mode, imc.mode) 32 | num += 1 33 | 34 | # Create compressed animated GIF file 35 | ofile = odir + "frames_0000_" + format_string("%04d", num) + ".gif" 36 | fp = open(ofile, "wb") 37 | gifmaker.makedelta(fp, seq) 38 | fp.close() 39 | 40 | -------------------------------------------------------------------------------- /MATLAB/avelE_wrt_jointm.m: -------------------------------------------------------------------------------- 1 | function [avel] = avelE_wrt_jointm(w,p,dq,m) 2 | 3 | % function [avel] = avelE_wrt_jointm(w,p,dq,m) : returns angular velocity of 4 | % line-of-sight for end-effector 5 | % with respect to joint m, 6 | % assuming the first joint 7 | % position is fixed in world 8 | % space. 9 | % w = set of n joint rotation direction vectors in world space 10 | % p = set of n+1 position vectors (n joints + end-effector) in world space 11 | % dq = vector of n joint rotation rates (radians/sec) 12 | % m = joint number of interest [1,n] 13 | 14 | %try 15 | % assert m-1 in range(len(dq)), 'value of m-1 not in range(len(dq))' 16 | %catch AssertionError as err 17 | % ename = err.__class__.__name__ 18 | % fname = 'avelE_wrt_jointm' 19 | % fprintf('%s in %s: %s\n', ename, fname, err) 20 | % sys.exit() 21 | 22 | n = length(dq); % number of joints 23 | ie = length(p); % index of end effector position vector 24 | 25 | % absolute velocity of end-effector 26 | vE = zeros(1,3); 27 | for i = n:-1:1 28 | vE = vE + cross(dq(i)*w{i},(p{i+1}-p{i})); 29 | end 30 | % absolute velocity of joint m 31 | vJ = zeros(1,3); 32 | for i = 1:1:m-1 33 | vJ = vJ + cross(dq(i)*w{i},(p{i+1}-p{i})); 34 | end 35 | % relative velocity of end-effector wrt joint m 36 | vEJ = vE - vJ; 37 | % direction vector to end-effector from joint m 38 | vecp = p{ie} - p{m}; 39 | nrmp = norm(vecp); 40 | % line-of-sight angular velocity for end-effector wrt joint m 41 | avel = cross(vecp,vEJ)/(nrmp*nrmp); 42 | 43 | end -------------------------------------------------------------------------------- /MATLAB/avelT_wrt_jointm.m: -------------------------------------------------------------------------------- 1 | function [avel] = avelT_wrt_jointm(pt,vt,w,p,dq,m) 2 | 3 | % function [avel] = avelT_wrt_jointm(pt,vt,w,p,dq,m) : returns angular velocity of 4 | % line-of-sight for the target 5 | % with respect to joint m, 6 | % assuming the first joint 7 | % position is fixed in world 8 | % space. 9 | % pt = current position of target in world space 10 | % vt = current velocity of target in world space 11 | % w = set of n joint rotation direction vectors in world space 12 | % p = set of n+1 position vectors (n joints + end-effector) in world space 13 | % dq = vector of n joint rotation rates (radians/sec) 14 | % m = joint number of interest [1,n] 15 | 16 | %try 17 | % assert m-1 in range(len(dq)), 'value of m-1 not in range(len(dq))' 18 | %catch AssertionError as err 19 | % ename = err.__class__.__name__ 20 | % fname = 'avelT_wrt_jointm' 21 | % fprintf('%s in %s: %s\n', ename, fname, err) 22 | % sys.exit() 23 | %end 24 | 25 | n = length(dq); % number of joints 26 | ie = length(p); % index of end effector position vector 27 | 28 | % absolute velocity of joint m 29 | vJ = zeros(1,3); 30 | for i = 1:1:m-1 31 | vJ = vJ + cross(dq(i)*w{i},(p{i+1}-p{i})); 32 | end 33 | % relative velocity of target wrt joint m 34 | vTJ = vt - vJ; 35 | % direction vector to target from joint m 36 | vecp = pt - p{m}; 37 | nrmp = norm(vecp); 38 | % line-of-sight angular velocity for target wrt joint m 39 | avel = cross(vecp,vTJ)/(nrmp*nrmp); 40 | 41 | end 42 | -------------------------------------------------------------------------------- /MATLAB/solve_chk.m: -------------------------------------------------------------------------------- 1 | function [i] = solve_chk(ni,ilim,p,ec,et,dxy,dz,derr,perr) 2 | 3 | % function [i] = solve_chk(ni,ilim,p,ec,et,dxy,dz,derr,perr) : returns 0 or 1 4 | % ni = number of iterations 5 | % ilim = iteration limit 6 | % p = set of n link joint position vectors 7 | % ec = end effector current position vector 8 | % et = end effector target position vector 9 | % dxy = length of fully extended link chain in xy plane 10 | % dz = lenght of fully extended link chain in z direction 11 | % derr = allowable effector to target distance error 12 | % perr = allowable effector to target pointing error 13 | 14 | i = 0; % assume effector has not reached target 15 | 16 | if ni == ilim 17 | % encountered iteration limit 18 | i = 1; 19 | else 20 | etp = et-p{1}; % vector from link chain base to target 21 | ecp = ec-p{1}; % vector from link chain base to effector 22 | detxy = norm(etp(1:2)); % distance from base to target in xy plane 23 | detz = abs(etp(3)); % distance from base to target in z direction 24 | decxy = norm(ecp(1:2)); % distance from base to effector in xy plane 25 | dotxy = dot(ecp(1:2),etp(1:2))/detxy; % portion of ecp along etp in xy plane 26 | 27 | if ((detxy - dxy) > derr) || ((detz - dz) > derr) 28 | % target currently beyond reach of effector; but is it 29 | % within allowable distance and xy pointing error? 30 | if (abs(decxy-dxy) <= derr) && (acos(dotxy/decxy) <= perr) 31 | i = 1; 32 | fprintf('Encountered done condition 1.\n') 33 | end 34 | else 35 | % target currently not beyond reach of effector; but is it 36 | % within allowable distance? 37 | if norm(et-ec) <= derr 38 | i = 1; 39 | fprintf('Encountered done condition 2.\n') 40 | end 41 | end 42 | 43 | end 44 | -------------------------------------------------------------------------------- /DISCLAIMER: -------------------------------------------------------------------------------- 1 | # IK_Solver -- Inverse Kinematics (IK) Solver 2 | # Copyright (c) 2015, Gary Deschaines 3 | # All rights reserved. 4 | # 5 | # These files included in this software package are part of IK_Solver. 6 | # 7 | # IK_Solver is free software: you can redistribute it and/or modify it 8 | # under the terms of the GNU Lesser General Public License as published 9 | # by the Free Software Foundation, either version 3 of the License, or 10 | # (at your option) any later version. You should have received a copy of 11 | # the GNU Leser General Public License (LICENSE-LGPLv3) along with the 12 | # IK_Solver source code. If not, see . 13 | # 14 | # Redistribution and use in source and binary forms, with or without 15 | # modification, are permitted provided that the following conditions 16 | # are met: 17 | # * Redistributions of source code must retain the above copyright 18 | # notice, this list of conditions and the following disclaimer. 19 | # * Redistributions in binary form must reproduce the above copyright 20 | # notice, this list of conditions and the following disclaimer in the 21 | # documentation and/or other materials provided with the distribution. 22 | # 23 | # THIS SOFTWARE IS PROVIDED BY GARY DESCHAINES ``AS IS'' AND ANY EXPRESS OR 24 | # IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 25 | # OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 26 | # IN NO EVENT SHALL GARY DESCHAINES BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 28 | # NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 29 | # DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 30 | # THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 31 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 32 | # THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 33 | -------------------------------------------------------------------------------- /.idea/markdown-navigator-enh.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /Blender/FourLink3D_BGE/scripts/moveEmpty.py: -------------------------------------------------------------------------------- 1 | # FILE: moveEmpty.py 2 | # PROG: fourlink3d_BGE.blend 3 | # DESC: Blender Game Engine logic block controller script for 4 | # keyboard control of Empty frame motion utilized as the 5 | # FourLink3D robotic manipulator arm end effector IK target. 6 | # 7 | # NOTE: Select layer 1 (solids) and layer 2 (armature) 8 | # in 3D View before pressing 'P' key to initiate 9 | # game engine. Press 'space' key to position the 10 | # empty target at the TargetPath start point. 11 | # 12 | # Control Keys 13 | # ------------ 14 | # 15 | # left arrow - increase target velocity along the 16 | # TargetPath curve in -Y direction. 17 | # right arrow - increase target velocity along the 18 | # TargetPath curve in +Y direction. 19 | # space bar - position target at start of the 20 | # TargetPath curve with zero velocity. 21 | 22 | import bge 23 | from mathutils import Vector 24 | 25 | # Acquire current controller and its owner object. 26 | cont = bge.logic.getCurrentController() 27 | obj = cont.owner 28 | 29 | # Acquire controller's keyboard keypress sensors. 30 | pressL = cont.sensors["left"] 31 | pressR = cont.sensors["right"] 32 | pressS = cont.sensors["space"] 33 | 34 | # Acquire controller's actuator and its linear velocity. 35 | move = cont.actuators["move"] 36 | velX = move.linV[0] 37 | velY = move.linV[1] 38 | velZ = move.linV[2] 39 | 40 | # Based on which keypress sensor exhibits a positive signal: 41 | # 42 | # 1) adjust (decr., incr. or zero) actuator linear velocity 43 | # 2) force immediate update of owner object linear velocity 44 | # 3) activate or deactivate the actuator 45 | 46 | if pressL.positive: 47 | # decrement world space linear velocity 48 | velX = velX - 0.0 49 | velY = velY - 0.2 50 | velZ = velZ - 0.0 51 | move.linV = Vector((velX, velY, velZ)) 52 | obj.setLinearVelocity(move.linV, False) 53 | cont.activate(move) 54 | 55 | elif pressR.positive: 56 | # increment world space linear velocity 57 | velX = velX + 0.0 58 | velY = velY + 0.2 59 | velZ = velZ + 0.0 60 | move.linV = Vector((velX, velY, velZ)) 61 | obj.setLinearVelocity(move.linV, False) 62 | cont.activate(move) 63 | 64 | elif pressS.positive: 65 | # reset world space linear velocity and position 66 | velX = 0.0 67 | velY = 0.0 68 | velZ = 0.0 69 | move.linV = Vector((velX, velY, velZ)) 70 | obj.setLinearVelocity(move.linV, False) 71 | obj.worldPosition = Vector((3.0, 4.0, 2.0)) 72 | cont.deactivate(move) 73 | 74 | -------------------------------------------------------------------------------- /MATLAB/null_space_control.m: -------------------------------------------------------------------------------- 1 | function [dH] = null_space_control(P3D,et,vt,w,p,dq,q,u) 2 | 3 | % function [dH] = null_space_control(P3D,et,vt,w,p,dq,q,u) : computes and returns 4 | % null space control vector 5 | % 6 | % NOTE: This function is tailored specifically to hardcoded n-link 7 | % chains defined in IK_Solver.m 8 | % 9 | % P3D = Plot3D 10 | % et = position of target in world space 11 | % vt = velocity of target in world space 12 | % w = set of n joint rotation direction vectors in world space 13 | % p = set of n+1 position vectors (n joints + end-effector) in world space 14 | % dq = vector of n joint rotation rates (radians/sec) 15 | % q = vector of joint rotations (radians) 16 | % u = set of joint rotation axis unit vectors 17 | 18 | global dpr; 19 | 20 | nq = length(dq); % number of link chain joints 21 | dH = zeros(1,nq); % zero control vector 22 | 23 | if P3D == 0 24 | %%fprintf('dq = %8.5f %8.5f %8.5f %8.5f\n',... 25 | %% dq(1),dq(2),dq(3),dq(5)) 26 | avT1 = avelT_wrt_jointm(et,vt,w,p,dq,1); 27 | avT2 = avelT_wrt_jointm(et,vt,w,p,dq,2); 28 | avT3 = avelT_wrt_jointm(et,vt,w,p,dq,3); 29 | avT5 = avelT_wrt_jointm(et,vt,w,p,dq,5); 30 | avT = [avT1(3), avT2(3), avT3(3), 0.0, avT5(3)]; 31 | %%fprintf('avT = %8.5f %8.5f %8.5f %8.5f\n',... 32 | %% avT(1),avT(2),avT(3),avT(5)) 33 | avE1 = avelE_wrt_jointm(w,p,dq,1); 34 | avE2 = avelE_wrt_jointm(w,p,dq,2); 35 | avE3 = avelE_wrt_jointm(w,p,dq,3); 36 | avE5 = avelE_wrt_jointm(w,p,dq,5); 37 | avE = [avE1(3), avE2(3), avE3(3), 0.0, avE5(3)]; 38 | %%fprintf('avE = %8.5f %8.5f %8.5f %8.5f\n',... 39 | %% avE(1),avE(2),avE(3),avE(5)) 40 | dH(1) = avE(1); 41 | dH(2) = avE(2); 42 | dH(3) = avE(3); 43 | %dH(5) = avE(5); 44 | 45 | phiZ = get_endeff_rot(nq,q,u); 46 | %%fprintf('phiZ = %8.5f\n', phiZ*dpr) 47 | dH(nq) = phiZ; 48 | else 49 | %%fprintf('dq = %8.5f %8.5f %8.5f\n', dq(1),dq(2),dq(3)) 50 | avT1 = avelT_wrt_jointm(et,vt,w,p,dq,1); 51 | avT2 = avelT_wrt_jointm(et,vt,w,p,dq,2); 52 | avT3 = avelT_wrt_jointm(et,vt,w,p,dq,3); 53 | avT = [avT1(3), avT2(2), avT3(2), 0.0, 0.0]; 54 | %%fprintf('avT = %8.5f %8.5f %8.5f\n', avT(1),avT(2),avT(3)) 55 | avE1 = avelE_wrt_jointm(w,p,dq,1); 56 | avE2 = avelE_wrt_jointm(w,p,dq,2); 57 | avE3 = avelE_wrt_jointm(w,p,dq,3); 58 | avE = [avE1(3), avE2(2), avE3(2), 0.0, 0.0]; 59 | %%fprintf('avE = %8.5f %8.5f %8.5f\n', avE(1),avE(2),avE(3)) 60 | dH(1) = avE(1); 61 | dH(2) = avE(2); 62 | dH(3) = avE(3); 63 | 64 | [psi,theta,phi] = get_endeff_ypr(nq,q,u); 65 | %%fprintf('psi,theta,phi = %8.5f, %8.5f, %8.5f\n',... 66 | %% psi*dpr,theta*dpr,phi*dpr) 67 | dH(nq-1) = -theta; 68 | dH(nq) = -psi; 69 | end 70 | 71 | end 72 | -------------------------------------------------------------------------------- /Blender/FourLink3D/scripts/addons/ArmatureRot.py: -------------------------------------------------------------------------------- 1 | # File: ArmatureRot.py 2 | # Date: 16 Apr 2015 3 | # Auth: Gary E. Deschaines 4 | # Desc: Blender script addon to set armature bone rotation angles. 5 | 6 | bl_info = { 7 | "name": "Rotate Bones", 8 | "category": "Object", 9 | } 10 | 11 | import bpy 12 | import mathutils as mu 13 | import math 14 | 15 | class ArmatureRot(bpy.types.Operator): 16 | """Armature Rotate Script""" # Blender will use this as a tooltip for 17 | # menu items and buttons. 18 | bl_idname = "armature.rot_bones" # unique identifier for buttons and menu 19 | # items to reference. 20 | bl_label = "Rotate Bones" # display name in the interface. 21 | bl_options = {'REGISTER', 'UNDO'} # enable undo for the operator. 22 | 23 | # New property to store angle value for the rotate bones tool. 24 | angle = bpy.props.IntProperty(name="RotAng", 25 | default=0, min=-180, max=180, 26 | subtype='ANGLE') 27 | 28 | # The execute() function is called by Blender when running the operator. 29 | def execute(self, context): 30 | # Rotate armature bones by value of angle. 31 | armature = context.scene.objects['Armature1'] 32 | for bone in armature.pose.bones: 33 | euler = bone.rotation_euler 34 | if bone.name == 'Bone1': 35 | euler.x = 0 36 | euler.y = math.radians(self.angle) 37 | euler.z = 0 38 | elif bone.name == 'Bone4': 39 | euler.x = math.radians(self.angle) 40 | euler.y = 0 41 | euler.z = math.radians(self.angle) 42 | else : 43 | euler.x = math.radians(self.angle) 44 | euler.y = 0 45 | euler.z = 0 46 | bone.rotation_euler = euler 47 | 48 | return {'FINISHED'} # this lets Blender know the operator finished 49 | # successfully. 50 | 51 | def menu_func(self, context): 52 | self.layout.operator(ArmatureRot.bl_idname) 53 | 54 | # Store keymaps here to access after registration. 55 | addon_keymaps = [] 56 | 57 | def register(): 58 | bpy.utils.register_class(ArmatureRot) 59 | bpy.types.VIEW3D_MT_object.append(menu_func) 60 | 61 | # Handle the keymap. 62 | wm = bpy.context.window_manager 63 | km = wm.keyconfigs.addon.keymaps.new(name='Object Mode', space_type='EMPTY') 64 | 65 | kmi = km.keymap_items.new(ArmatureRot.bl_idname, 'SPACE', 'PRESS', 66 | ctrl=True, shift=True) 67 | kmi.properties.angle = 5 68 | addon_keymaps.append((km, kmi)) 69 | 70 | def unregister(): 71 | bpy.utils.unregister_class(ArmatureRot) 72 | bpy.types.VIEW3D_MT_object.remove(menu_func) 73 | 74 | # Handle the keymap. 75 | for km, kmi in addon_keymaps: 76 | km.keymap_items.remove(kmi) 77 | addon_keymaps.clear() 78 | 79 | 80 | # This allows you to run the script directly from Blender's text 81 | # editor to test the addon without having to install it. 82 | if __name__ == "__main__": 83 | register() 84 | -------------------------------------------------------------------------------- /util/gifmaker.py: -------------------------------------------------------------------------------- 1 | # 2 | # The Python Imaging Library 3 | # $Id: gifmaker.py 2134 2004-10-06 08:55:20Z fredrik $ 4 | # 5 | # convert sequence format to GIF animation 6 | # 7 | # history: 8 | # 97-01-03 fl created 9 | # 10 | # Copyright (c) Secret Labs AB 1997. All rights reserved. 11 | # Copyright (c) Fredrik Lundh 1997. 12 | # 13 | # See the README file for information on usage and redistribution. 14 | # 15 | 16 | # 17 | # For special purposes, you can import this module and call 18 | # the makedelta or compress functions yourself. For example, 19 | # if you have an application that generates a sequence of 20 | # images, you can convert it to a GIF animation using some- 21 | # thing like the following code: 22 | # 23 | # import Image 24 | # import gifmaker 25 | # 26 | # sequence = [] 27 | # 28 | # # generate sequence 29 | # for i in range(100): 30 | # im = 31 | # sequence.append(im) 32 | # 33 | # # write GIF animation 34 | # fp = open("out.gif", "wb") 35 | # gifmaker.makedelta(fp, sequence) 36 | # fp.close() 37 | # 38 | # Alternatively, use an iterator to generate the sequence, and 39 | # write data directly to a socket. Or something... 40 | # 41 | 42 | from PIL import Image, ImageChops 43 | 44 | from PIL.GifImagePlugin import getheader, getdata 45 | 46 | # -------------------------------------------------------------------- 47 | # sequence iterator 48 | 49 | class image_sequence: 50 | def __init__(self, im): 51 | self.im = im 52 | def __getitem__(self, ix): 53 | try: 54 | if ix: 55 | self.im.seek(ix) 56 | return self.im 57 | except EOFError: 58 | raise IndexError # end of sequence 59 | 60 | # -------------------------------------------------------------------- 61 | # straightforward delta encoding 62 | 63 | def makedelta(fp, sequence): 64 | """Convert list of image frames to a GIF animation file""" 65 | 66 | frames = 0 67 | 68 | previous = None 69 | 70 | for im in sequence: 71 | 72 | # 73 | # FIXME: write graphics control block before each frame 74 | 75 | if not previous: 76 | 77 | # global header 78 | for s in getheader(im)[0] + getdata(im): 79 | fp.write(s) 80 | 81 | else: 82 | 83 | # delta frame 84 | delta = ImageChops.subtract_modulo(im, previous) 85 | 86 | bbox = delta.getbbox() 87 | 88 | if bbox: 89 | 90 | # compress difference 91 | for s in getdata(im.crop(bbox), offset = bbox[:2]): 92 | fp.write(s) 93 | 94 | else: 95 | # FIXME: what should we do in this case? 96 | pass 97 | 98 | previous = im.copy() 99 | 100 | frames = frames + 1 101 | 102 | fp.write(";".encode()) 103 | 104 | return frames 105 | 106 | # -------------------------------------------------------------------- 107 | # main hack 108 | 109 | def compress(infile, outfile): 110 | 111 | # open input image, and force loading of first frame 112 | im = Image.open(infile) 113 | im.load() 114 | 115 | # open output file 116 | fp = open(outfile, "wb") 117 | 118 | seq = image_sequence(im) 119 | 120 | makedelta(fp, seq) 121 | 122 | fp.close() 123 | 124 | 125 | if __name__ == "__main__": 126 | 127 | import sys 128 | 129 | if len(sys.argv) < 3: 130 | print "GIFMAKER -- create GIF animations" 131 | print "Usage: gifmaker infile outfile" 132 | sys.exit(1) 133 | 134 | compress(sys.argv[1], sys.argv[2]) 135 | -------------------------------------------------------------------------------- /.idea/markdown-navigator.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 21 | 22 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /Blender/README.txt: -------------------------------------------------------------------------------- 1 | The ./FourLink3D and ./FourLink3D_BGE subdirectories contain Blender files and 2 | associated Python scripts to model and render the motion of a three dimensional, 3 | five revolute joint, 4-link serial chain defined in the IK_Solver_nlink.py script 4 | which represents a notional robotic manipulator arm. 5 | 6 | The fourlink3d.blend, fourlink3d_BGE.blend files and associated Python scripts were 7 | originally developed on a MS Windows 7 Pro operating system with Blender 2.68 and 8 | Python 3.3, and have been successfully used with Ubuntu operating system, Blender 9 | and Python versions listed in the following table. 10 | 11 | Ubuntu Blender Python 12 | ------ ------- ------ 13 | 14.04 2.74 3.4.2 14 | 16.04 2.74 3.4.2 15 | 18.04 2.79 3.7.0 (Blender Game Engine provided by UPBGE v0.2.4b) 16 | 18.04 2.81 3.7.4 (Blender Game Engine no longer in Blender v2.8x) 17 | 18 | FourLink3D.blend - inverse kinematics solved using the IK_Solver program 19 | ------------------------------------------------------------------------ 20 | 21 | The robot model's initial pose is specified by the IK_Solver class methods, and its 22 | motion is determined by applying Damped Least Squares algorithm with null space 23 | control in response to movement of the end-effector target modeled as a blue sphere 24 | linked to an Empty frame object which is constrained to move along a straight line 25 | path starting at the XYZ point [3.0, 2.5, 2.0] and parallel to the world space Y axis. 26 | 27 | The ./FourLink3D/scripts/IK_SolverPanel.py script is utilized to create a Blender 28 | control panel; providing means for Blender to connect as a client with an executing 29 | IK_Solver_server.py program in order to control inverse kinematics of the armature 30 | skeleton rig for the Blender robot model as described in the ./FourLink3D/README.txt 31 | file. 32 | 33 | FourLink3D_BGE.blend - inverse kinematics solved with Blender's IK solvers 34 | -------------------------------------------------------------------------- 35 | 36 | The robot model's initial pose is specified by the user selected Blender's built-in 37 | IK solver method and specified parameters, and its motion is determined by applying 38 | the IK solver algorithm in response to movement of the end-effector target modeled as 39 | a blue sphere linked to an Empty frame object which is constrained to move along a 40 | straight line path parallel to the world space Y axis; starting at the XYZ point 41 | [3.0, 4.0, 2.0] and ending at the point [3.0, -4.0, 2.0]. 42 | 43 | In Blender Render mode, motion of the end effector target along its NURB curve path 44 | is controlled by animation keyframe buttons in a Timeline editor panel. The frame 45 | count from 0 to 500 as the target moves from its initial to final position along 46 | the target path represents 40 seconds at 12.5 FPS; which corresponds to a target 47 | velocity of 0.2 units/seconds (8 units traversed in 40 seconds). 48 | 49 | The ./FourLink3D_BGE/scripts/timeText.py script is run once while in Blender Render 50 | mode to register a handler for animating frame time text in the 3D viewport. 51 | 52 | In Blender Game mode with Blender Game Engine activated, motion of the end effector 53 | target is controlled by user keypresses as specified in the game logic editor panel 54 | associated with the Empty object and as documented in the moveEmpty.py script. 55 | 56 | The ./FourLink3D_BGE/scripts/moveEmpty.py script is utilized in Blender Game mode 57 | to control the motion of an Empty object assigned as the target for the armature 58 | end effector as directed by user keypresses. Each left or right arrow keypress 59 | adjusts the target velocity by -/+ 0.2 units/second respectively. Pressing the 60 | spacebar resets the target's initial XYZ location to [3.0, 4.0, 2.0] with zero 61 | linear velocity. 62 | -------------------------------------------------------------------------------- /IK_Solver_nlink.py: -------------------------------------------------------------------------------- 1 | # File: IK_Solver_nlink.py 2 | # Auth: G. E. Deschaines 3 | # Date: 17 Apr 2015 4 | # Prog: Inverse Kinematics (IK) Solver n-link chain definition. 5 | # Desc: Functions utilized by the IK_Solver class, server and client. 6 | # 7 | # This file provides the n-link chain parameters definition function 8 | # utilized by the IK_Solver_server and IK_Solver_client modules. 9 | # 10 | # Disclaimer: 11 | # 12 | # See DISCLAIMER 13 | 14 | import sys 15 | 16 | try: 17 | from IK_Solver_funcs import * 18 | except ImportError: 19 | print("* Error: IK_Solver_funcs.py required.") 20 | sys.exit() 21 | 22 | def get_nlink_chain(Plot3D) : 23 | """ 24 | Jointed n-Link Chain Pictogram 25 | + 26 | uz0 uz1 uz2 uzN-1 . (a)N 27 | | uy0 | uy1 | uy2 | uyN-1 . ^end 28 | | / | / | / | / . | effector 29 | |/ |/ |/ |/ . | 30 | base>+------>ux0 -+------>ux1 -+------>ux2 ... -+------>uxN-1 | 31 | (a)0 (a)1 (a)2 (a)N-1 | 32 | ^joint_1 ^joint_2 ^joint_3 ^joint_N | 33 | | & link_1 | & link_2 | & link_3 | & link_N | 34 | | | | | | 35 | +----< preceding link body space [x,y,z] >---+ 36 | """ 37 | if Plot3D == 0 : 38 | # 2D 4-link chain 39 | a = {0:np.float64([0.0,0.0,0.0]), # <- joint 1 + set of link chain attachment 40 | 1:np.float64([2.0,0.0,0.0]), # <- joint 2 | points in body coordinates, 41 | 2:np.float64([2.0,0.0,0.0]), # <- joint 3 | except for the base given in 42 | 3:np.float64([1.0,0.0,0.0]), # <- joint 4 | world space coordinates 43 | 4:np.float64([0.0,0.0,0.0]), # <- joint 5 / 44 | 5:np.float64([1.0,0.0,0.0])} # <- end effector 45 | u = {0:np.float64([0.0,0.0,1.0]), # <- joint 1 + set of joint rotation axis 46 | 1:np.float64([0.0,0.0,1.0]), # <- joint 2 | unit vectors in link body 47 | 2:np.float64([0.0,0.0,1.0]), # <- joint 3 | coordinates 48 | 3:np.float64([1.0,0.0,0.0]), # <- joint 4 | 49 | 4:np.float64([0.0,0.0,1.0])} # <- joint 5 / 50 | q0 = np.float64([ 25*rpd, # <- joint 1 + initial joint rotations (radians) 51 | 15*rpd, # <- joint 2 | 52 | 10*rpd, # <- joint 3 | 53 | 0*rpd, # <- joint 4 | 54 | 5*rpd]) # <- joint 5 / 55 | qmin = np.float64([-360*rpd, # <- joint 1 + joint minimum rotations (radians) 56 | -135*rpd, # <- joint 2 | 57 | -60*rpd, # <- joint 3 | 58 | -360*rpd, # <- joint 4 | 59 | -160*rpd]) # <- joint 5 / 60 | qmax = np.float64([ 360*rpd, # <- joint 1 + joint maximum rotations (radians) 61 | 135*rpd, # <- joint 2 | 62 | 60*rpd, # <- joint 3 | 63 | 360*rpd, # <- joint 4 | 64 | 160*rpd]) # <- joint 5 / 65 | dqlim = np.float64([ 30*rpd, # <- joint 1 + joint delta rotation limit (rad/sec) 66 | 30*rpd, # <- joint 2 | 67 | 30*rpd, # <- joint 3 | 68 | 30*rpd, # <- joint 4 | 69 | 30*rpd]) # <- joint 5 / 70 | else: 71 | # 3D 4-link chain 72 | a = {0:np.float64([0.0,0.0,0.0]), # <- joint 1 + set of link chain attachment 73 | 1:np.float64([0.0,0.0,2.0]), # <- joint 2 | points in body coordinates, 74 | 2:np.float64([2.0,0.0,0.0]), # <- joint 3 | except for the base given in 75 | 3:np.float64([2.0,0.0,0.0]), # <- joint 4 | world space coordinates 76 | 4:np.float64([0.0,0.0,0.0]), # <- joint 5 / 77 | 5:np.float64([0.5,0.0,0.0])} # <- end effector 78 | u = {0:np.float64([0.0,0.0,1.0]), # <- joint 1 + set of joint rotation axis 79 | 1:np.float64([0.0,1.0,0.0]), # <- joint 2 | unit vectors in link body 80 | 2:np.float64([0.0,1.0,0.0]), # <- joint 3 | coordinates 81 | 3:np.float64([0.0,1.0,0.0]), # <- joint 4 | 82 | 4:np.float64([0.0,0.0,1.0])} # <- joint 5 / 83 | q0 = np.float64([ 0*rpd, # <- joint 1 + initial joint rotations (radians) 84 | -45*rpd, # <- joint 2 | 85 | 45*rpd, # <- joint 3 | 86 | 0*rpd, # <- joint 4 | 87 | 45*rpd]) # <- joint 5 / 88 | qmin = np.float64([-360*rpd, # <- joint 1 + joint minimum rotations (radians) 89 | -135*rpd, # <- joint 2 | 90 | -135*rpd, # <- joint 3 | 91 | -85*rpd, # <- joint 4 | 92 | -120*rpd]) # <- joint 5 / 93 | qmax = np.float64([ 360*rpd, # <- joint 1 + joint maximum rotations (radians) 94 | 135*rpd, # <- joint 2 | 95 | 135*rpd, # <- joint 3 | 96 | 85*rpd, # <- joint 4 | 97 | 120*rpd]) # <- joint 5 / 98 | dqlim = np.float64([ 30*rpd, # <- joint 1 + joint delta rotation limit (rad/sec) 99 | 30*rpd, # <- joint 2 | 100 | 30*rpd, # <- joint 3 | 101 | 30*rpd, # <- joint 4 | 102 | 30*rpd]) # <- joint 5 / 103 | 104 | return (a, u, q0, qmin, qmax, dqlim) 105 | -------------------------------------------------------------------------------- /LICENSE-LGPLv3: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. 166 | -------------------------------------------------------------------------------- /Blender/FourLink3D/README.txt: -------------------------------------------------------------------------------- 1 | File: README.txt 2 | Auth: G. E. Deschaines 3 | Date: 19 Apr 2015 4 | Prog: Blender fourlink3d robot with IK_Solver python scripts. 5 | Desc: Instructions on using Blender with fourlink3d.blend 6 | and IK_Solver python scripts. 7 | 8 | For this Blender model, the motion of a 3D four link serial chain 9 | (actually 5 links but only 4 visible) comprised of revolute joints 10 | defined in the IK_Solver/IK_Solver_nlink.py file is determined by 11 | application of "Damped Least Squares" algorithm from the initial 12 | manipulator arm orientation to that obtained when the end effector 13 | is placed sufficiently close to the predicted intercept point of a 14 | moving target represented by a blue sphere linked to an empty frame. 15 | 16 | These instructions are for using Blender as the rendering interface 17 | to the IK_Solver program. The IK_Solver program can be executed with 18 | a matplotlib graphics interface by following instructions in the 19 | IK_Solver/README.txt file. 20 | 21 | 1) Start the IK_Solver Server in a terminal window as: 22 | 23 | > python ../../IK_Solver_server.py 24 | 25 | Should see "waiting for connection..." displayed in the 26 | terminal window. 27 | 28 | 2) Start Blender with the fourlink3d.blend file in a 29 | terminal window as: 30 | 31 | > blender fourlink3d.blend 32 | 33 | 3) To create keyframes of robot motion in Blender. 34 | 35 | (A) The middle-left window pane must be a "3D View" type. 36 | (B) The 3D-View mode must be in "Object Mode". 37 | (C) The active (selected) object must be "Armature1". The 38 | quickest way to select "Armature1" is by using the 39 | upper right window pane, which should be a "Outliner" 40 | type. "Armature1" is a sub-object of "Scene" and by 41 | clicking the + button to the left of the "Scene" label 42 | the hierarchy of connected objects will be displayed. 43 | If the "Armature1" label is not high-lighted in white, 44 | a left click on "Armature1" should select it, indicated 45 | by a white high-lighted label. 46 | (D) Press "Ctrl-Alt-U" keys to bring up the "User Preferences" 47 | window and check the "Auto Run Python Scripts" box on the 48 | "File" tab page. 49 | (E) Change the lower-left window pane to "Text Editor" type. 50 | (F) The "IK_SolverPanel.py" script should be displayed in 51 | the active text box and "Register" should be checked. 52 | (G) Click on the "Run Script" button. 53 | (H) The "IK Solver Server Tools" menu pane should become 54 | visible at the bottom of the lower right window pane, 55 | which should be displaying data properties for 56 | "Armature1" if steps (A), (B) and (C) were satisfied and 57 | the armature icon (stick-man) is high-lighted. 58 | (I) Click on the "IK Solver Start Operator" button to initiate 59 | a connection to the IK_Solver Server and create frame 0. 60 | Should see the robot move to it's initial state in the 3D 61 | View window pane, and the following messages displayed in 62 | the terminal window in which the IK_Solver Server was 63 | started: 64 | ...connected from: 127.0.0.1:port# 65 | Starting IK_Solver (Client3D=1,IKmethod=7) 66 | The last message line should also be displayed in the 67 | terminal window in which Blender was started. 68 | (J) Click on "IK Solver Update Operator" button to solve inverse 69 | kinematics from initial manipulator arm orientation to end 70 | effector contact with moving empty frame. 71 | (K) Click on "IK Solver Stop Operator" button to disconnect from 72 | the IK_Solver Server, or click on the "IK Solver Server Quit" 73 | button to stop the IK_Solver Server before closing the socket 74 | connection to the server. 75 | 76 | 4) To view keyframes of robot motion in Blender. 77 | 78 | (A) The middle-left window pane must be a "3D View" type. 79 | (B) The 3D-View mode must be "Object Mode". 80 | (C) The active (selected) object should be "Armature1" if 81 | Layer 1 is selected. 82 | (D) Change the lower-left window pane to "Timeline" type. 83 | (E) Use the play, stop, frame advance buttons to see the robot's 84 | movement in the 3D View window pane. You can change view point 85 | while in Object or Pose mode using the numeric keypad as such: 86 | 5 - Toggle between orthographic/perspective mode 87 | 1 - Look down the +Y global axis (forward view) 88 | 3 - Look down the -X global axis (right side view) 89 | 7 - Look down the -Z global axis (top view) 90 | + - Zoom in 91 | - - Zoom out 92 | 0 - Use active camera 93 | Ctrl-0 - Use selected camera as active camera 94 | You can also use the mouse to pan, tilt, shift and zoom. 95 | (F) To create an animation of robot manipulator arm IK motion as 96 | viewed by the camera, switch the lower right window from scene 97 | "Armature1" skeleton pose (stick-man) mode to scene Render 98 | (camera) mode and select the "Animation" button. 99 | (G) Motion paths can be added for the end effector and target by 100 | selecting each object in the 3D viewport's "Object Mode" and 101 | enter "Calculate" in the "Animation" tabbed panel. Alternately, 102 | each object may be selected in the upper right "Outliner" editor 103 | window and "Update Paths" selected under Motion Path" in the 104 | lower right "Properties" editor window if in object mode. Be sure 105 | to set the frame limits to match those presented in the bottom 106 | "Timeline" editor window. 107 | (H) To clear armature and empty animations, click on "Animation" 108 | under "Armature1" and "Empty" in the upper left "Outliner" editor 109 | window using the right mouse button and select "Clear Animation 110 | Data." 111 | 112 | NOTES: 113 | 114 | 1) The global reference frame in Blender and IK_Solver are right-handed 115 | XYZ coordinate systems, the rest orientation of the robot arm is not 116 | equivalently specified. In Blender the robot armature rest position 117 | is defined in the YZ plane, while in IK_Solver it is defined in the 118 | XZ as shown in the following pictograms. 119 | 120 | +Z Bone 2 +Z Link 2 121 | ^ | Joint 3 ^ | Joint 3 122 | | |-X /Bone 3 End-Effector | |+Y /Link 3 End-Effector 123 | | |/ / | | | |/ / | | 124 | Joint 2-O====O======O===< Joint 2-O====O======O===< 125 | Bone 1--I/ \ \ Link 1--I/ \ \ 126 | O-------> +Y \ Bone 4 O-------> +X \ Links 4&5 127 | / \ Joint 4 / \ Joints 4&5 128 | / Joint 1 / Joint 1 129 | +X -Y 130 | 131 | Robot Arm in Blender Robot Arm in IK_Solver 132 | 133 | Movement in the -X and +Y directions in Blender is movement in +Y 134 | and +X directions respectively in IK_Solver. 135 | 136 | 2) Joint rotations in Blender are defined in local xyz reference 137 | frames for each bone as shown in the following pictogram. 138 | 139 | +z (2) +z (3) +z (4) The bone rotation axes: 140 | |/ |/ |/ (1) About +y axis 141 | O->+y==O->+y====O->+y=< (2) About +x axis 142 | /I / / (3) About +x axis 143 | +x I +x +x (4) About +x and +z axes. 144 | +y 145 | ^ (1) 146 | |/ 147 | +z<--O------->+Y 148 | / \ 149 | +x Armature Base 150 | / 151 | +X 152 | 153 | Positive rotation about the +x axis is in the opposite direction 154 | to rotation about the +y axis in IK_Solver. 155 | 156 | 3) Joint rotations in IK_Solver are defined in local xyz reference 157 | frames for each link segment as shown in the following pictogram. 158 | 159 | +z +y +z +y +z +y The joint rotation axes: 160 | |/ |/ |/ 1) About +z axis 161 | O->+x==O->+x====O->+x=< 2) About +y axis 162 | /I / / 3) About +y axis 163 | Joint 2 I Joint 3 Joints 4&5 4) About +y axis 164 | +z +y and +Y 5) About +z axis 165 | |/ 166 | O-->+x and +X 167 | / 168 | Joint 1 169 | 170 | Positive rotation about the +y axis is in the opposite direction 171 | to rotation about the +x axis in Blender. 172 | 173 | 4) The target's position and velocity are updated based on frame number 174 | and sent to the IK_Solver by calls to sendTgtPos() and sendTgtVel() 175 | functions respectively from the IK_SolverUpdate() function in the 176 | IK_SolverPanel.py file. 177 | -------------------------------------------------------------------------------- /IK_Solver_server.py: -------------------------------------------------------------------------------- 1 | # File: IK_Solver_server.py 2 | # Auth: G. E. Deschaines 3 | # Date: 17 Apr 2015 4 | # Prog: Inverse Kinematics (IK) Solver server 5 | # Desc: Applies user selected IK solving technique to determine and 6 | # plot the motion of a jointed n-link chain in 2D or 3D space. 7 | # 8 | # This Python program presents a 3D implementation of inverse kinematic 9 | # solutions to end effector movement for a n-link, revolute-joint chain 10 | # towards a target using cyclic coordinate descent (CCD) [1], Jacobian 11 | # transpose method (JTM) [3], pseudo-inverse method (PIM) [1,2,3] or 12 | # damped least squares (DLS) [3] as detailed in the listed references. 13 | # 14 | # The program source is comprised of the following Python files. 15 | # 16 | # IK_Solver_server.py - main routine and socket connection functions 17 | # IK_Solver_nlink.py - IK solver n-link chain definition function 18 | # IK_Solver_class.py - IK solver iteration and state data accessors 19 | # IK_Solver_funcs.py - IK solver math methods and support functions 20 | # 21 | # The IK solution technique is user selectable, and the program can be 22 | # configured to record the plotted link chain motion to a video file 23 | # for animation playback (requires ffmpeg or avconv). 24 | # 25 | # References: 26 | # 27 | # [1] Benjamin Kenwright, "Practical 01: Inverse Kinematics", September 28 | # 2014, School of Computer Science, Edinburgh Napier University, 29 | # United Kingdom, Physics-Based Animation practical web available at 30 | # http://games.soc.napier.ac.uk/study/pba_practicals/Practical%2001%20-%20Inverse%20Kinematics.pdf 31 | # 32 | # [2] Ben Kenwright, "Real-Time Character Inverse Kinematics using 33 | # the Gauss-Seidel Iterative Approximation Method", July 2012, 34 | # CONTENT 2012, The Fourth International Conference on Creative 35 | # Content Technologies, web available at 36 | # http://www.thinkmind.org/index.php?view=article&articleid=content_2012_4_10_60013 37 | # 38 | # [3] Samuel R. Buss, "Introduction to Inverse Kinematics with Jacobian 39 | # Transpose, Pseudoinverse and Damped Least Squares methods", October 40 | # 2009, Department of Mathematics, University of California, San 41 | # Diego, unpublished article web available at 42 | # http://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/iksurvey.pdf 43 | # 44 | # Disclaimer: 45 | # 46 | # See DISCLAIMER 47 | 48 | import sys 49 | import os 50 | import time 51 | 52 | from math import ceil 53 | from socket import * 54 | from array import * 55 | from struct import * 56 | 57 | try: 58 | import numpy as np 59 | import scipy.linalg as la 60 | except ImportError: 61 | print("* Error: NumPy and SciPy packages required.") 62 | print(" Suggest installing the SciPy stack.") 63 | sys.exit() 64 | 65 | try: 66 | from IK_Solver_nlink import * 67 | from IK_Solver_class import * 68 | except ImportError: 69 | print("* Error: IK_Solver_class.py required.") 70 | sys.exit() 71 | 72 | def pack_points_xy(buffer,ioff,n,X,Y): 73 | 74 | pack_into('I',buffer,ioff,n) 75 | ioff = ioff + calcsize('I') 76 | for i in range(0,n) : 77 | pack_into('ff',buffer,ioff,X[i],Y[i]) 78 | ioff = ioff + calcsize('ff') 79 | return ioff 80 | 81 | def pack_points_xyz(buffer,ioff,n,X,Y,Z): 82 | 83 | pack_into('I',buffer,ioff,n) 84 | ioff = ioff + calcsize('I') 85 | for i in range(0,n) : 86 | pack_into('fff',buffer,ioff,X[i],Y[i],Z[i]) 87 | ioff = ioff + calcsize('fff') 88 | return ioff 89 | 90 | def pack_joints_rot(buffer,ioff,n,Q): 91 | 92 | pack_into('I',buffer,ioff,n) 93 | ioff = ioff + calcsize('I') 94 | for i in range(0,n) : 95 | pack_into('f',buffer,ioff,Q[i]) 96 | ioff = ioff + calcsize('f') 97 | return ioff 98 | 99 | def unpack_mouse_bxy(buffer,ioff): 100 | 101 | (b,) = unpack_from('I',buffer,ioff) 102 | ioff = ioff + calcsize('I') 103 | (x,y)= unpack_from('ff',buffer,ioff) 104 | ioff = ioff + calcsize('ff') 105 | return (ioff, b, x, y) 106 | 107 | def unpack_target_pos(buffer,ioff): 108 | (x,y,z)= unpack_from('fff',buffer,ioff) 109 | ioff = ioff + calcsize('fff') 110 | return (ioff, x, y, z) 111 | 112 | def unpack_target_vel(buffer,ioff): 113 | (vx,vy,vz)= unpack_from('fff',buffer,ioff) 114 | ioff = ioff + calcsize('fff') 115 | return (ioff, vx, vy, vz) 116 | 117 | def ik_update2D(i): 118 | """ 119 | IK solver update function for 2D plotting 120 | """ 121 | global BUFSIZ, ik_solver, tdel 122 | 123 | ik_solver.step(tdel) 124 | if i == 0 : 125 | ik_solver.zero() 126 | 127 | # Get data from IK solver 128 | time = ik_solver.tsolve 129 | (X0,Y0) = ik_solver.get_points_x0y0() 130 | (X,Y) = ik_solver.get_points_xy() 131 | (XT,YT) = ik_solver.get_target_xy() 132 | (XPT,YPT) = ik_solver.get_predtgt_xy() 133 | # Flush buffer 134 | buffer = array('B',(0 for i in range(0,BUFSIZ))) 135 | 136 | # Pack data to send to client 137 | ioff = 0 138 | pack_into('f',buffer,ioff,time) 139 | ioff = ioff + calcsize('f') 140 | n = np.size(X0) 141 | ioff = pack_points_xy(buffer,ioff,n,X0,Y0) 142 | n = np.size(X) 143 | ioff = pack_points_xy(buffer,ioff,n,X,Y) 144 | n = np.size(XT) 145 | ioff = pack_points_xy(buffer,ioff,n,XT,YT) 146 | n = np.size(XPT) 147 | ioff = pack_points_xy(buffer,ioff,n,XPT,YPT) 148 | return (ioff, buffer) 149 | 150 | def ik_update3D(i): 151 | """ 152 | IK solver update function for 3D plotting 153 | """ 154 | global BUFSIZ, ik_solver, tdel 155 | 156 | ik_solver.step(tdel) 157 | if i == 0 : 158 | ik_solver.zero() 159 | 160 | # Get data from IK solver 161 | time = ik_solver.tsolve 162 | (X0,Y0,Z0) = ik_solver.get_points_x0y0z0() 163 | (X,Y,Z) = ik_solver.get_points_xyz() 164 | (XT,YT,ZT) = ik_solver.get_target_xyz() 165 | (XE,YE,ZE) = ik_solver.get_endeff_xyz() 166 | (XPT,YPT,ZPT) = ik_solver.get_predtgt_xyz() 167 | 168 | # Flush buffer 169 | buffer = array('B',(0 for i in range(0,BUFSIZ))) 170 | 171 | # Pack data to send to client 172 | ioff = 0 173 | pack_into('f',buffer,ioff,time) 174 | ioff = ioff + calcsize('f') 175 | n = np.size(X0) 176 | ioff = pack_points_xyz(buffer,ioff,n,X0,Y0,Z0) 177 | n = np.size(X) 178 | ioff = pack_points_xyz(buffer,ioff,n,X,Y,Z) 179 | n = np.size(XT) 180 | ioff = pack_points_xyz(buffer,ioff,n,XT,YT,ZT) 181 | n = np.size(XE) 182 | ioff = pack_points_xyz(buffer,ioff,n,XE,YE,ZE) 183 | n = np.size(XPT) 184 | ioff = pack_points_xyz(buffer,ioff,n,XPT,YPT,ZPT) 185 | return (ioff, buffer) 186 | 187 | def ik_updateBL(i): 188 | """ 189 | IK solver update function for Blender animation 190 | """ 191 | global BUFSIZ, ik_solver, tdel 192 | 193 | ik_solver.step(tdel) 194 | if i == 0 : 195 | ik_solver.zero() 196 | 197 | # Get data from IK solver 198 | time = ik_solver.tsolve 199 | (XT,YT,ZT) = ik_solver.get_target_xyz() 200 | (XE,YE,ZE) = ik_solver.get_endeff_xyz() 201 | (Q) = ik_solver.get_joints_rot() 202 | 203 | # Flush buffer 204 | buffer = array('B',(0 for i in range(0,BUFSIZ))) 205 | 206 | # Pack data to send to client 207 | ioff = 0 208 | pack_into('f',buffer,ioff,time) 209 | ioff = ioff + calcsize('f') 210 | n = np.size(XT) 211 | ioff = pack_points_xyz(buffer,ioff,n,XT,YT,ZT) 212 | n = np.size(XE) 213 | ioff = pack_points_xyz(buffer,ioff,n,XE,YE,ZE) 214 | n = np.size(Q) 215 | ioff = pack_joints_rot(buffer,ioff,n,Q) 216 | 217 | return (ioff, buffer) 218 | 219 | # IK_Solver server main. 220 | 221 | if __name__ == '__main__': 222 | 223 | # Processing selectors 224 | 225 | IKmethod = 1 # selected IK solver method 226 | Plot3D = 0 # plot in 3D flag 227 | 228 | # TCP/IP sockets parameters 229 | 230 | HOST = '127.0.0.1' 231 | PORT = 21000 232 | ADDR = (HOST, PORT) 233 | BUFSIZ = 512 234 | 235 | buffer = array('B',(0 for i in range(0,BUFSIZ))) 236 | 237 | tcpSerSock = socket(family=AF_INET, type=SOCK_STREAM) 238 | tcpSerSock.bind(ADDR) 239 | tcpSerSock.listen(5) 240 | 241 | FPS = 10 242 | tdel = 1.0/FPS 243 | ik_solver = 0 244 | 245 | # Main processing loop over client connections 246 | 247 | quit = False 248 | while not quit: 249 | print('waiting for connection...') 250 | tcpCliSock, addr = tcpSerSock.accept() 251 | print("...connected from: %s:%d" % (addr[0], addr[1])) 252 | # Client session initiated. 253 | while True : 254 | raw_data = tcpCliSock.recv(BUFSIZ) 255 | if not raw_data : 256 | break 257 | #print('recv:', raw_data) 258 | if raw_data[0:5] == 'start'.encode('utf-8') : 259 | data = raw_data.decode('utf-8').strip() 260 | (s, p1, p2) = data.split(' ',2) 261 | Plot3D = int(p1) 262 | IKmethod = int(p2) 263 | # define link chain structure and parameters. 264 | (a, u, q0, qmin, qmax, dqlim) = get_nlink_chain(Plot3D) 265 | # instantiate an IK_Solver. 266 | ik_solver = IK_Solver(Plot3D,IKmethod,a,u,q0,qmin,qmax,dqlim) 267 | data = 'Starting IK_Solver (Plot3D=' + p1 + ',IKmethod=' + p2 + ')' 268 | print(data) 269 | tcpCliSock.send(data.encode()) 270 | elif raw_data[0:6] == 'update'.encode('utf-8') : 271 | data = raw_data.decode('utf-8').strip() 272 | (s, p1) = data.split(' ',1) 273 | i = int(p1) 274 | if Plot3D == 0 : 275 | (n, data) = ik_update2D(i) 276 | elif Plot3D == 1 : 277 | (n, data) = ik_update3D(i) 278 | elif Plot3D == 2 : 279 | (n, data) = ik_updateBL(i) 280 | tcpCliSock.send(data) 281 | elif raw_data[0] == 'M'.encode('utf-8')[0] : 282 | (ioff,b,x,y) = unpack_mouse_bxy(raw_data,1) 283 | ik_solver.mouse_click(b,x,y) 284 | data = 'ack' 285 | tcpCliSock.send(data.encode()) 286 | elif raw_data[0] == 'P'.encode('utf-8')[0] : 287 | (ioff,x,y,z) = unpack_target_pos(raw_data,1) 288 | ik_solver.set_target_pos(x,y,z) 289 | data = 'ack' 290 | tcpCliSock.send(data.encode()) 291 | elif raw_data[0] == 'V'.encode('utf-8')[0] : 292 | (ioff,vx,vy,vz) = unpack_target_vel(raw_data,1) 293 | ik_solver.set_target_vel(vx,vy,vz) 294 | data = 'ack' 295 | tcpCliSock.send(data.encode()) 296 | elif raw_data[0:4] == 'stop'.encode('utf-8') : 297 | print('Stopping IK_Solver') 298 | del ik_solver 299 | ik_solver = 0 300 | break 301 | elif raw_data[0:4] == 'quit'.encode('utf-8') : 302 | print('Stopping IK_Solver') 303 | del ik_solver 304 | ik_solver = 0 305 | quit = True 306 | break 307 | else: 308 | print('Unknown message received from client:', raw_data) 309 | raw_data = '' 310 | 311 | # Client session concluded. 312 | tcpCliSock.close() 313 | if ik_solver != 0 : 314 | print('Stopping IK_Solver') 315 | del ik_solver 316 | ik_solver = 0 317 | 318 | # Terminate processing and exit. 319 | 320 | tcpSerSock.close() 321 | -------------------------------------------------------------------------------- /Blender/FourLink3D/scripts/IK_SolverPanel.py: -------------------------------------------------------------------------------- 1 | # File: IK_SolverPanel.py 2 | # Data: 26 Apr 2015 3 | # Auth: Gary E. Deschaines 4 | # Desc: Blender panel tool for connecting to an IK_Solver server. 5 | # 6 | # Disclaimer: 7 | # 8 | # See IK_Solver/DISCLAIMER 9 | 10 | import bpy 11 | import mathutils as mu 12 | import math 13 | 14 | from socket import * 15 | from array import * 16 | from struct import * 17 | 18 | ### IK Server tcp connection parameters 19 | 20 | HOST = '127.0.0.1' 21 | PORT = 21000 22 | ADDR = (HOST, PORT) 23 | BUFSIZ = 512 24 | tcpCliSock = 0 25 | nFrame = 0 26 | lastTime = -1.0 27 | targetLoc = mu.Vector((0.0,0.0,0.0)) 28 | endeffLoc = mu.Vector((0.0,0.0,0.0)) 29 | 30 | ### Time text display handler 31 | 32 | scene = bpy.context.scene 33 | ttobj = scene.objects['Text_Time'] 34 | 35 | def time_text(scene): 36 | fps = scene.render.fps / scene.render.fps_base # actual framerate 37 | ttobj.data.body = 'Time: {0: 6.3f} secs'.format(scene.frame_current/fps) 38 | 39 | bpy.app.handlers.frame_change_pre.append(time_text) 40 | 41 | ### Data pack/unpack functions 42 | 43 | def pack_target_pos(buffer,x,y,z): 44 | ioff = 0 45 | pack_into('c',buffer,ioff,'P'.encode()) 46 | ioff = ioff + calcsize('c') 47 | pack_into('fff',buffer,ioff,x,y,z) 48 | ioff = ioff + calcsize('fff') 49 | return ioff 50 | 51 | def pack_target_vel(buffer,vx,vy,vz): 52 | ioff = 0 53 | pack_into('c',buffer,ioff,'V'.encode()) 54 | ioff = ioff + calcsize('c') 55 | pack_into('fff',buffer,ioff,vx,vy,vz) 56 | ioff = ioff + calcsize('fff') 57 | return ioff 58 | 59 | def unpack_target_loc(buffer,ioff): 60 | (n,) = unpack_from('I',buffer,ioff) 61 | ioff = ioff + calcsize('I') 62 | Loc = [] 63 | for i in range(0,n) : 64 | (x, y, z) = unpack_from('fff',buffer,ioff) 65 | ioff = ioff + calcsize('fff') 66 | Loc.append(mu.Vector((x, y ,z))) 67 | return (ioff, Loc) 68 | 69 | def unpack_endeff_loc(buffer,ioff): 70 | (n,) = unpack_from('I',buffer,ioff) 71 | ioff = ioff + calcsize('I') 72 | Loc = [] 73 | for i in range(0,n) : 74 | (x, y, z) = unpack_from('fff',buffer,ioff) 75 | ioff = ioff + calcsize('fff') 76 | Loc.append(mu.Vector((x, y ,z))) 77 | return (ioff, Loc) 78 | 79 | def unpack_joints_rot(buffer,ioff): 80 | (n,) = unpack_from('I',buffer,ioff) 81 | ioff = ioff + calcsize('I') 82 | Rot = [] 83 | for i in range(0,n) : 84 | (q,)= unpack_from('f',buffer,ioff) 85 | ioff = ioff + calcsize('f') 86 | Rot.append(q) 87 | return (ioff, Rot) 88 | 89 | ### Functions utilized by the IK_SolverUpdate operator 90 | 91 | def sendTgtVel(n,context): 92 | global BUFSIZ, tcpCliSock 93 | # Update target velocity 94 | if n == 0 : 95 | tVel = mu.Vector((0.0,-0.2, 0.0)) 96 | elif n == 251: 97 | tVel = mu.Vector((0.0, 0.0, 0.0)) 98 | # Flush buffer. 99 | buffer = array('B',(0 for i in range(0,16))) 100 | # Pack target velocity and send. 101 | ioff = pack_target_vel(buffer,tVel[0],tVel[1],tVel[2]) 102 | tcpCliSock.send(buffer) 103 | # Wait for acknowledge. 104 | data = tcpCliSock.recv(16) 105 | 106 | def sendTgtPos(n,context): 107 | global BUFSIZ, tcpCliSock 108 | # Get empty object. 109 | empty = bpy.data.objects['Empty'] 110 | eLoc = empty.location 111 | # Calculate and send target position. 112 | if n == 0 : 113 | tPos = eLoc 114 | # Flush buffer. 115 | buffer = array('B',(0 for i in range(0,16))) 116 | # Pack target position and send. 117 | ioff = pack_target_pos(buffer,tPos[0],tPos[1],tPos[2]) 118 | tcpCliSock.send(buffer) 119 | # Wait for acknowledge. 120 | data = tcpCliSock.recv(16) 121 | eLoc[1] = 2.5 - 0.2*(n*0.08) 122 | empty.location = eLoc 123 | empty.keyframe_insert(data_path="location",frame=n,index=1) 124 | 125 | def updateIK(n, armature): 126 | global BUFSIZ, tcpCliSock, lastTime, targetLoc, endeffLoc 127 | 128 | # Send update message with frame # to server and wait to receive data. 129 | data = 'update' + ' ' + str(n) 130 | tcpCliSock.send(data.encode()) 131 | buffer = tcpCliSock.recv(BUFSIZ) 132 | # Unpack data received. 133 | ioff = 0 134 | (time,)= unpack_from('f',buffer,ioff) 135 | ioff = ioff + calcsize('f') 136 | (ioff,tLoc) = unpack_target_loc(buffer,ioff) 137 | (ioff,eLoc) = unpack_endeff_loc(buffer,ioff) 138 | (ioff,jRot) = unpack_joints_rot(buffer,ioff) 139 | 140 | # Store target and end-effector locations. 141 | targetLoc[0] = tLoc[0][0] 142 | targetLoc[1] = tLoc[0][1] 143 | targetLoc[2] = tLoc[0][2] 144 | endeffLoc[0] = eLoc[0][0] 145 | endeffLoc[1] = eLoc[0][1] 146 | endeffLoc[2] = eLoc[0][2] 147 | ''' 148 | print("time=%8.3f tLoc=[%8.4f %8.4f %8.4f] eLoc=[%8.4f %8.4f %8.4f]" % \ 149 | (time, targetLoc.x, targetLoc.y, targetLoc.z, \ 150 | endeffLoc.x, endeffLoc.y, endeffLoc.z) ) 151 | ''' 152 | # Check for IK Solver iterations completed. 153 | if abs(time-lastTime) < 0.001 : return True 154 | lastTime = time 155 | 156 | # Update orientations of armature bones. 157 | k = 0 158 | for bone in armature.pose.bones: 159 | euler = bone.rotation_euler 160 | if bone.name == 'Bone1': 161 | euler.x = 0 162 | euler.y = jRot[k] 163 | euler.z = 0 164 | bone.rotation_euler = euler 165 | bone.keyframe_insert(data_path="rotation_euler",frame=n,index=1) 166 | elif bone.name == 'Bone4': 167 | k = k + 1 168 | euler.x = -jRot[k] 169 | euler.y = 0 170 | euler.z = 0 171 | bone.rotation_euler = euler 172 | bone.keyframe_insert(data_path="rotation_euler",frame=n,index=0) 173 | k = k + 1 174 | euler.x = 0 175 | euler.y = 0 176 | euler.z = jRot[k] 177 | bone.rotation_euler = euler 178 | bone.keyframe_insert(data_path="rotation_euler",frame=n,index=2) 179 | else : 180 | k = k + 1 181 | euler.x = -jRot[k] 182 | euler.y = 0 183 | euler.z = 0 184 | bone.rotation_euler = euler 185 | bone.keyframe_insert(data_path="rotation_euler",frame=n,index=0) 186 | # Return indication IK Solver iterations not completed. 187 | return False 188 | 189 | ### IK_Solver tool operators 190 | 191 | class IK_SolverUpdate(bpy.types.Operator): 192 | """Tooltip""" 193 | bl_idname = "armature.ik_solver_update" 194 | bl_label = "IK Solver Update Operator" 195 | 196 | @classmethod 197 | def poll(cls, context): 198 | return context.active_object is not None 199 | 200 | def execute(self, context): 201 | global tcpCliSock, nFrame 202 | if tcpCliSock : 203 | # Move armature. 204 | done = False 205 | while not done : 206 | nFrame = nFrame + 1 207 | if nFrame <= 251 : sendTgtPos(nFrame, context) 208 | if nFrame == 251 : sendTgtVel(nFrame, context) 209 | done = updateIK(nFrame, context.active_object) 210 | return {'FINISHED'} 211 | 212 | class IK_SolverStop(bpy.types.Operator): 213 | """Tooltip""" 214 | bl_idname = "armature.ik_solver_stop" 215 | bl_label = "IK Solver Stop Operator" 216 | 217 | @classmethod 218 | def poll(cls, context): 219 | return context.active_object is not None 220 | 221 | def execute(self, context): 222 | global tcpCliSock, nFrame 223 | if tcpCliSock : 224 | data = 'stop' 225 | tcpCliSock.send(data.encode()) 226 | tcpCliSock.close() 227 | del tcpCliSock 228 | tcpCliSock = 0 229 | return {'FINISHED'} 230 | 231 | class IK_SolverStart(bpy.types.Operator): 232 | """Tooltip""" 233 | bl_idname = "armature.ik_solver_start" 234 | bl_label = "IK Solver Start Operator" 235 | 236 | @classmethod 237 | def poll(cls, context): 238 | return context.active_object is not None 239 | 240 | def execute(self, context): 241 | global tcpCliSock, nFrame, lastTime 242 | if tcpCliSock == 0 : 243 | tcpCliSock = socket(family=AF_INET, type=SOCK_STREAM) 244 | try : 245 | nFrame = 0 246 | lastTime = -1.0 247 | Plot3D = 2 248 | IKmethod = 7 249 | tcpCliSock.connect(ADDR) 250 | data = 'start' + ' ' + str(Plot3D) + ' ' + str(IKmethod) 251 | tcpCliSock.send(data.encode()) 252 | raw_data = tcpCliSock.recv(BUFSIZ) 253 | data = raw_data.decode() 254 | print(data) 255 | sendTgtPos(nFrame, context) 256 | sendTgtVel(nFrame, context) 257 | updateIK(nFrame, context.active_object) 258 | except error : 259 | print('* Could not connect to the IK Solver server.') 260 | print(' The IK Solver server needs to be running.') 261 | if tcpCliSock : 262 | tcpCliSock.close() 263 | del tcpCliSock 264 | tcpCliSock = 0 265 | return {'FINISHED'} 266 | 267 | class IK_SolverQuit(bpy.types.Operator): 268 | """Tooltip""" 269 | bl_idname = "armature.ik_solver_quit" 270 | bl_label = "IK Solver Quit Operator" 271 | 272 | @classmethod 273 | def poll(cls, context): 274 | return context.active_object is not None 275 | 276 | def execute(self, context): 277 | global tcpCliSock 278 | if tcpCliSock : 279 | data = 'quit' 280 | tcpCliSock.send(data.encode()) 281 | tcpCliSock.close() 282 | del tcpCliSock 283 | tcpCliSock = 0 284 | return {'FINISHED'} 285 | 286 | ### IK_Solver menu panel 287 | 288 | class ARMATURE_PT_IK_SolverPanel(bpy.types.Panel): 289 | bl_label = "IK Solver Server" 290 | bl_space_type = 'PROPERTIES' 291 | bl_region_type = 'WINDOW' 292 | bl_context = "data" 293 | bl_options = {'DEFAULT_CLOSED'} 294 | 295 | @classmethod 296 | def poll(cls, context): 297 | return (context.object is not None and \ 298 | context.active_object.name in bpy.data.armatures.keys()) 299 | 300 | def draw(self, context): 301 | layout = self.layout 302 | obj = context.object 303 | box = layout.box() 304 | box.label(text="IK Solver Server Tools") 305 | box.operator("armature.ik_solver_start") 306 | box.operator("armature.ik_solver_update") 307 | box.operator("armature.ik_solver_stop") 308 | box.operator("armature.ik_solver_quit") 309 | 310 | ### Menu panel and operator register/unregister functions 311 | 312 | def register(): 313 | bpy.utils.register_class(IK_SolverStart) 314 | bpy.utils.register_class(IK_SolverUpdate) 315 | bpy.utils.register_class(IK_SolverStop) 316 | bpy.utils.register_class(IK_SolverQuit) 317 | bpy.utils.register_class(ARMATURE_PT_IK_SolverPanel) 318 | 319 | def unregister(): 320 | bpy.utils.register_class(ARMATURE_PT_IK_SolverPanel) 321 | bpy.utils.unregister_class(IK_SolverQuit) 322 | bpy.utils.unregister_class(IK_SolverStop) 323 | bpy.utils.unregister_class(IK_SolverUpdate) 324 | bpy.utils.unregister_class(IK_SolverStart) 325 | 326 | if __name__ == "__main__": 327 | register() 328 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## IK_Solver ## 2 | IK_Solver - Inverse Kinematic Solver for n-link serial chain 3 | 4 | The interactive IK_Solver program graphically demonstrates implementation of 5 | several inverse kinematic solving techniques, as presented in the referenced documents, 6 | for two and three-dimensional, revolute joint, n-link serial chains. The program 7 | is written entirely in version 2 and 3 compatible [Python](https://www.python.org/) 8 | and requires [SciPy](https://www.scipy.org/), [NumPy](https://www.numpy.org/) and [Matplotlib](https://matplotlib.org/) 9 | packages. A [MATLAB®](https://www.mathworks.com/products/matlab.html)/[Octave](https://www.gnu.org/software/octave/) 10 | script variant is also provided in the ./MATLAB subdirectory of this distribution. 11 | For both Python and MATLAB®/Octave variants, a video encoder application, such as 12 | [ffmpeg](https://ffmpeg.org/) or avconv from [Libav](https://github.com/libav/libav), 13 | is required to record graphic animations. 14 | 15 | The following images and hyperlinked video clips depict the motion of two-dimensional 16 | (2D) and three-dimensional (3D) five revolute joint, 4-link serial chains (blue line) 17 | as the end-effector moves from its initial position (green line) towards a moving target 18 | (red x). Although 14 simulation seconds (140 frames \@ 10FPS) are shown, it only took 19 | 2.52 and 5.44 seconds for the 2D and 3D link chains respectively to reach the target. In 20 | both cases the end-effector moves to a predicted intercept point (magenta x) along the 21 | target's path of motion. Joint rotation clamping and rotation rate limiting are utilized 22 | to provide a more realistic simulation of the link chain's movement. Even though only 23 | four links are apparent in the serial chains shown, both actually have five links. A 24 | zero length link separates two orthogonal revolute joints between the last two visible 25 | links; the last link considered as the end-effector. 26 | 27 | | 2D 4-Link Frame 0 | 2D 4-Link Frame 140 | 28 | |-----------------------------------------------------------------------------------------------------------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------| 29 | | ./docs/images/IK_Solver_3_2D_001.jpg | ./docs/images/IK_Solver_3_2D_140.jpg | 30 | 31 | | 3D 4-Link Frame 0 | 3D 4-Link Frame 140 | 32 | |-----------------------------------------------------------------------------------------------------------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------| 33 | | ./docs/images/IK_Solver_3_3D_001.jpg | ./docs/images/IK_Solver_3_3D_140.jpg | 34 | 35 | If animation recording is not selected, then during 2D link chain animation the user can move 36 | the cursor to a location within the drawing area and press the middle mouse button to interrupt 37 | IK solving and place the target at the picked location. The link chain end-effector will then 38 | be directed toward intercepting the target as it moves from its new location. For non-recorded 39 | 3D link chain animation, pressing the middle mouse button causes the target to move to a randomly 40 | generated position -- left and right button presses combined with mouse motion will rotate, zoom 41 | and pan the 3D view space as determined by the Matplotlib figure manager and backend renderer. 42 | IK solving and link chain animation will stop when one of the following conditions occurs: 43 | 1) the end-effector reaches the target (within 4% of the effector's length), 44 | 2) it's determined the end-effector cannot reach the target, or 45 | 3) 30 seconds has elapsed. 46 | 47 | Closing the graphic animation window will terminate processing and the program will exit. 48 | 49 | #### Getting Started #### 50 | 51 | The IK_Solver program can be executed in two different configurations, 52 | as a single executable program, or as client/server programs. 53 | 54 | 1. To invoke as a single program, execute the following command in 55 | a terminal window. 56 | 57 | > python IK_Solver.py $1 $2 $3 58 | 59 | where the command line arguments $1, $2 and $3 can be as follows. 60 | 61 | $1 - The IK solver method option \[1,2,3,4,5,6,7] where: 62 | 63 | 1 = Cyclic Coordinate Descent (CCD) 64 | 2 = Jacobian Transpose Method (JTM) 65 | 3 = Pseudo-inverse Jacobian method 2 (PIM2) 66 | 4 = Pseudo-inverse Jacobian method 3 (PIM3) 67 | 5 = Damped Least Squares (DLS) 68 | 6 = PIM3 solver with null space control 69 | 7 = DLS solver with null space control 70 | 71 | $2 - The reference frame and plotting mode (0=2D, 1=3D). 72 | 73 | $3 - The animation recording switch (0=Off, 1=On). Note that 74 | animation recording requires ffmpeg or avconv. 75 | 76 | 2. To invoke as a client/server programs, first start the server 77 | program in a terminal window using the following command. 78 | 79 | > python IK_Solver_server.py 80 | 81 | Next, start the client program in another terminal window using 82 | the command: 83 | 84 | > python IK_Server_client.py $1 $2 $3 85 | 86 | where the command line arguments are the same as those presented 87 | above for the single executable program. 88 | 89 | The n-link chain is defined in the IK_Solver_nlink.py file using displacement and 90 | rotation direction vectors, and not with Denavit-Hartenberg (DH) parameters. The 91 | target's initial position and fixed velocity are hard-coded within animate2D and 92 | animate3D functions in IK_Solver.py, and as IK_solver class attributes et0 93 | and vt0 respectively in the IK_Solver_class.py file. For a 2D case, the pseudo-inverse 94 | null space may be used to coerce the orientation of the end-effector link segment to be 95 | pointing in the +X direction for the PIM3 and DLS methods. Similarly, for the 3D case 96 | the null space may be used to coerce the end-effector link segment to be pointing in the 97 | +X direction. 98 | 99 | #### IK_Solver and Blender #### 100 | 101 | The utility of the IK_Solver client/server configuration will become apparent when 102 | it's demonstrated how IK_Solver can be used to drive an armature skeleton rigged 103 | 3D model of a robotic manipulator arm in [Blender](https://www.blender.org)! See 104 | [README.txt](./Blender/FourLink3D/README.txt) file under the ./Blender/FourLink3D 105 | subdirectory for instructions on using Blender 2.74 to animate the representative 106 | 3D 4-Link robotic manipulator arm illustrated in Matplotlib stick figure images 107 | above and as shown in Blender rendered images below. Clicking on the rendered images 108 | will link to a YouTube video or animated GIF image depicting robotic arm motion from 109 | initial pose (frame 0) to target intercept (frame 97). 110 | 111 | | Blender Rendered 3D 4-Link Frame 0 | Blender Rendered 3D 4-Link Frame 97 | 112 | |-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| 113 | | ./docs/images/Blender_IK_Solver_7_3D_00.png | ./docs/images/Blender_IK_Solver_7_3D_97.png | 114 | 115 | | Blender OpenGL Render 3D 4-Link Frame 0 | Blender OpenGL Render 3D 4-Link Frame 97 | 116 | |-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| 117 | | ./docs/images/Blender_IK_Solver_7_GL_00.png | ./docs/images/Blender_IK_Solver_7_GL_97.png | 118 | 119 | #### Further Attention #### 120 | 121 | * The interactive feature of the program is disabled when animation recording 122 | is selected. If frame images are saved after each figure drawing update, then 123 | the interactive feature may not have to be deactivated, and a video 124 | could be assembled from the sequence of saved images as a post-process. 125 | * The name and location of the saved video file is not currently a user specifiable option. 126 | * Instead of the target velocity being hard coded, it could be generated by a 127 | parametric function or read from a trajectory file. 128 | * Animation functions in IK_Solver.py and IK_Solver_client.py could be consolidated 129 | into a class to eliminate duplicative code. 130 | * Enhancements to target intercept prediction could include the earliest possible intercept, 131 | or smoothing and filtering to reduce prediction error. 132 | * Python logging could be used instead of verbose print statements 133 | being manually uncommented/commented for debug, test and operational phases 134 | of the program's development and maintenance. 135 | 136 | #### References #### 137 | 138 | \[1] Benjamin Kenwright, "Practical 01: Inverse Kinematics", September 139 | 2014, School of Computer Science, Edinburgh Napier University, 140 | United Kingdom, Physics-Based Animation practical web available at 141 | 142 | (2016 publication: ) 143 | 144 | \[2] Ben Kenwright, "Real-Time Character Inverse Kinematics using 145 | the Gauss-Seidel Iterative Approximation Method", July 2012, 146 | CONTENT 2012, The Fourth International Conference on Creative 147 | Content Technologies, web available at 148 | 149 | 150 | \[3] Samuel R. Buss, "Introduction to Inverse Kinematics with Jacobian 151 | Transpose, Pseudoinverse and Damped Least Squares methods", October 152 | 2009, Department of Mathematics, University of California, San 153 | Diego, unpublished article web available at 154 | 155 | 156 | These reference documents are provided in the ./refs subdirectory in the event 157 | their associated web links are broken. For additional reference information, 158 | software, demos and tutorials on physics-based animation, inverse kinematics 159 | and 3D computer graphics see Ben Kenwright's [XBDEV.NET](http://www.xbdev.net/physics/Book/index.php) 160 | website, Edinburgh Napier University Games Development [Resources](http://games.soc.napier.ac.uk/resources.html), and Sam Buss's [Publications and Other Research](http://www.math.ucsd.edu/~sbuss/ResearchWeb/index.html) web page. 161 | 162 | #### Disclaimers #### 163 | 164 | * See the file [DISCLAIMER](./DISCLAIMER) 165 | 166 | #### Epilogue #### 167 | 168 | Python purists and the omnipotent PEP 8 style and naming conventions enforcement 169 | agents will no doubt take extreme umbrage to the IK_Solver program source code. 170 | For those grown accustomed to using IDEs with GUIs, editors with color and layout 171 | schemes, automated code inspection and refactoring, computers with wide screen 172 | high-def monitors, gigabytes of memory and storage, and gigahertz CPUs/GPUs, 173 | I present the user interface of my early programming experience. 174 | 175 | https://commons.wikimedia.org/wiki/File:Hollerith_card.jpg 176 | 177 | Electromechanical typing and only 80 characters; tabbing, spacing and verbose statements 178 | were a luxury, not a necessity. And no, that's not me or my dad in the YouTube 179 | video presented when you click on the Hollerith card image. 180 | 181 | The IK_Solver program's primary role is as a simple demonstration and learning tool for 182 | inverse kinematic algorithms, animated graphics and client/server communication. 183 | It is not intended to be representative of commercial software engineering 184 | practices. With the aforementioned capabilities and features of modern software 185 | development platforms, users of IK_Solver are well-equipped to tailor the source 186 | code to their needs and whims. 187 | 188 | Respectfully, 189 | Gary E. Deschaines 190 | -------------------------------------------------------------------------------- /IK_Solver_funcs.py: -------------------------------------------------------------------------------- 1 | # File: IK_Solver_funcs.py 2 | # Auth: G. E. Deschaines 3 | # Date: 27 Apr 2015 4 | # Prog: Inverse Kinematics (IK) Solver functions 5 | # Desc: Functions utilized by the IK_Solver class. 6 | # 7 | # This file provides the following math methods and support functions 8 | # utilized by the IK_Solver class. 9 | # 10 | # rotation - compute coordinate rotation matrix using quaternion math 11 | # vT - perform vector transpose of NumPy 1D arrays 12 | # transform - transform coordinates from link body to world space 13 | # jacobian - compute Jacobian matrix (see IK_Solve.py ref [1,2]) 14 | # ik_jtm - apply IK Jacobian transpose method (see IK_Solve.py ref [3]) 15 | # ik_pim2 - apply IK pseudo-inverse method (see ref [2]) 16 | # ik_pim3 - apply IK pseudo-inverse method (see ref [3]) 17 | # ik_dls - apply IK damped least squares technique (see ref [3]) 18 | # clamp_rot - clamp joint rotations to specified limits 19 | # angle_chk - debug display of joint rotation angle checks 20 | # 21 | # time_to_goal - estimates time for end-effector to reach the 22 | # target goal 23 | # closing_gain - closing gain for end effector motion toward 24 | # target 25 | # avelT_wrt_jointm - angular velocity of the target with respect 26 | # to joint m 27 | # avelE_wrt_jointm - angular velocity of end-effector with respect 28 | # to joint m 29 | # 30 | # Disclaimer: 31 | # 32 | # See DISCLAIMER 33 | 34 | import sys 35 | 36 | from math import pi 37 | 38 | try: 39 | import numpy as np 40 | import scipy.linalg as la 41 | except ImportError: 42 | print("* Error: NumPy and SciPy packages required.") 43 | print(" Suggest installing the SciPy stack.") 44 | sys.exit() 45 | 46 | rpd = pi/180 # radians per degree conversion constant 47 | dpr = 180/pi # degrees per radian conversion constant 48 | 49 | p360rad = 360*rpd # globals used in clamp_rot function 50 | n360rad = -360*rpd # 51 | p180rad = 180*rpd # 52 | n180rad = -180*rpd # 53 | 54 | # Compute coordinate transformation rotation matrix 55 | def rotation(r,u): 56 | """ 57 | function R = rotation(r,u) : returns local to global rotation matrix 58 | r = joint rotation (radians) 59 | u = joint rotation axis unit vector 60 | """ 61 | theta = r/2 62 | costh = np.cos(theta) 63 | sinth = np.sin(theta) 64 | 65 | q0 = costh 66 | q1 = sinth*u[0] 67 | q2 = sinth*u[1] 68 | q3 = sinth*u[2] 69 | 70 | q00 = q0*q0 71 | q01 = q0*q1 72 | q02 = q0*q2 73 | q03 = q0*q3 74 | q11 = q1*q1 75 | q12 = q1*q2 76 | q13 = q1*q3 77 | q22 = q2*q2 78 | q23 = q2*q3 79 | q33 = q3*q3 80 | 81 | R = np.array([[q00+q11-q22-q33, 2*(q12-q03), 2*(q13+q02)], 82 | [2*(q12+q03), q00-q11+q22-q33, 2*(q23-q01)], 83 | [2*(q13-q02), 2*(q23+q01), q00-q11-q22+q33]]) 84 | return R 85 | 86 | # Vector Transpose for 1D array 87 | def vT(v) : 88 | """ 89 | function v^T = vT(v) : returns the transpose of a vector 90 | v = vector stored in a (1,n) or (n,1) array 91 | """ 92 | s = np.shape(v) 93 | if np.size(s) == 1 : 94 | return np.reshape(v,(s[0],-1)) 95 | else : 96 | return np.ravel(v) 97 | 98 | # Transform points and rotations from link body to world space 99 | def transform(n,r,u,a): 100 | """ 101 | function (p,w) = transform(n,r,u,a) : transforms a and u to p and w 102 | r = vector of n-1 joint rotations (radians) 103 | u = set of n-1 joint rotation axis unit vectors 104 | a = set of n link joint attachment location vectors to transform 105 | """ 106 | # 107 | # Transform point (a) on link body to point (p) in world space 108 | p = a.copy() 109 | for i in range(n) : 110 | p[i] = a[i] 111 | j = i - 1 112 | while j >= 0 : 113 | R = rotation(r[j],u[j]) 114 | p[i] = vT(np.dot(R,vT(p[i]))) 115 | p[i] = p[i] + a[j] 116 | j = j - 1 117 | # 118 | # Transform rotation axis (u) of link body to axis (w) in world space 119 | w = u.copy() 120 | for i in range(n-1) : 121 | j = i 122 | while j >= 0 : 123 | R = rotation(r[j],u[j]) 124 | w[i] = vT(np.dot(R,vT(w[i]))) 125 | w[i] = w[i]/la.norm(w[i]) 126 | j = j - 1 127 | return (p,w) 128 | 129 | # Compute Jacobian matrix J of de/dq to solve de=J*dq for dq. 130 | def jacobian(n,r,p,e): 131 | """ 132 | function J = jacobian(n,r,p,e) : returns de/dq Jacobian matrix for 133 | e representing a 1x3 vector in 134 | world space [x,y,z] coordinates 135 | r = set of n joint rotation direction vectors (i.e., a set 136 | {r1, r2, r3, ... rn} where r's are vectors of the form 137 | [rx,ry,rz]). 138 | p = set of at least n-1 link joint position vectors (i.e., 139 | {p1, p2, p3, ...} where p's are vectors of the form 140 | [px,py,pz]). 141 | e = desired effector position vector (i.e., [ex,ey,ez]). 142 | """ 143 | J = np.zeros((3,n),dtype=np.float64) 144 | for i in range(n) : 145 | j = np.cross(r[i],e-p[i]) # de/dq = r x (e - p) 146 | J[0,i] = j[0] # | dex/dq1 dex/dq2 dex/dq3 ... dex/dqn | 147 | J[1,i] = j[1] # | dey/dq1 dey/dq2 dey/dq3 ... dey dqn | 148 | J[2,i] = j[2] # | dez/dq1 dez/dq2 dez/dq3 ... dez/dqn | 149 | return J 150 | 151 | # IK Jacobian Transpose method to solve dq=inv(J)*de as presented 152 | # in reference [3] 153 | def ik_jtm(J,de,dqlim): 154 | """ 155 | function dq = ik_jtm(J,de,dqlim) : solves de = J*dq for dq using 156 | the Jacobian transpose method 157 | J = Jacobian matrix of de/dq 158 | de = vector of delta errors 159 | dqlim = vector of joint rotation delta limits 160 | """ 161 | dq = np.dot(np.transpose(J),vT(de)) 162 | jdq = np.dot(J,dq) 163 | alpha = np.dot(de,jdq)/pow(la.norm(np.transpose(jdq)),2) 164 | dqmax = np.amax(np.absolute(dq)) 165 | beta = dqlim[0]/dqmax 166 | dq = np.amin([alpha,beta])*vT(dq) 167 | return dq 168 | 169 | # IK Pseudo-Inverse Method to solve dq=inv(J)*de as presented in 170 | # reference [2] 171 | def ik_pim2(J,de,sdel,dqlim): 172 | """ 173 | function dq = ik_pim2(J,de,sdel,dqlim) : solves de = J*dq for dq using 174 | the pseudo-inverse method from 175 | reference [2] 176 | J = Jacobian matrix of de/dq 177 | de = vector of delta errors 178 | sdel = singularity prevention damping constant 179 | dqlim = vector of joint rotation delta limits 180 | """ 181 | n = np.size(J,1) 182 | A = np.dot(np.transpose(J),J) + sdel*np.eye(n) 183 | b = np.dot(np.transpose(J),vT(de)) 184 | dq = vT(np.dot(la.inv(A),b)) 185 | dqmax = np.amax(np.absolute(dq)) 186 | if dqmax > dqlim[0] : 187 | dq = (dqlim[0]/dqmax)*dq 188 | return dq 189 | 190 | # IK Pseudo-inverse Method to solve dq=inv(J)*de as presented in 191 | # reference [3] 192 | def ik_pim3(J,de,sfac,dqlim,phi): 193 | """ 194 | function dq = ik_pim3(J,de,sfac,dqlim,phi) : solves de = J*dq for dq using 195 | the pseudo-inverse method from 196 | reference [3]. 197 | J = Jacobian matrix of de/dq 198 | de = vector of delta errors 199 | sfac = singularity threshold factor 200 | dqlim = vector of joint rotation delta limits 201 | phi = null space control vector 202 | """ 203 | try: 204 | U,s,Vh = la.svd(J, full_matrices=True) # NOTE: Vh here is V' in MATLAB 205 | except la.LinAlgError : 206 | print("SVD computation does not converge.") 207 | 208 | (m,n) = J.shape 209 | """ 210 | %S = np.zeros((m,n),dtype=np.float64) 211 | for i in range(m) : 212 | S[i,i] = s[i] 213 | assert np.allclose(J, np.dot(U, np.dot(S, V))) 214 | """ 215 | slim = sfac*np.amax(np.absolute(s)) 216 | Sinv = np.zeros((n,m),dtype=np.float64) 217 | for i in range(m) : 218 | if abs(s[i]) > slim : 219 | Sinv[i,i] = 1.0/s[i] 220 | 221 | Jinv = np.dot(np.transpose(Vh),np.dot(Sinv,np.transpose(U))) 222 | Jprj = np.eye(n,dtype=np.float64) - np.dot(Jinv,J) 223 | dq = np.dot(Jinv,vT(de)) + np.dot(Jprj,vT(phi)) 224 | dqmax = np.amax(np.absolute(dq)) 225 | for i in range(len(dq)) : 226 | if dqmax > dqlim[i] : 227 | dq[i] = dq[i]*(dqlim[i]/dqmax) 228 | return vT(dq) 229 | 230 | # IK Damped Least Squares technique to solve dq=inv(J)*de as presented 231 | # in reference [3] 232 | def ik_dls(J,de,slam,dqlim,phi): 233 | """ 234 | function dq = ik_dls(J,de,slam,dqlim,phi) : solves de = J*dq for dq using 235 | damped least squares with SVD 236 | from reference [3]. 237 | J = Jacobian matrix of de/dq 238 | de = vector of delta errors 239 | slam = singularity damping factor 240 | dqlim = vector of joint rotation delta limits 241 | phi = null space control vector 242 | """ 243 | try: 244 | U,s,Vh = la.svd(J,full_matrices=True) # NOTE: Vh here is V' in MATLAB 245 | except la.LinAlgError : 246 | print("SVD computation does not converge.") 247 | 248 | (m,n) = J.shape 249 | """ 250 | S = np.zeros((m,n),dtype=np.float64) 251 | for i in range(m) : 252 | S[i,i] = s[i] 253 | assert np.allclose(J, np.dot(U, np.dot(S, V))) 254 | """ 255 | Sinv = np.zeros((n,m),dtype=np.float64) 256 | for i in range(m) : 257 | Sinv[i,i] = s[i]/(pow(s[i],2) + pow(slam,2)) 258 | 259 | Jinv = np.dot(np.transpose(Vh),np.dot(Sinv,np.transpose(U))) 260 | Jprj = np.eye(n,dtype=np.float64) - np.dot(Jinv,J) 261 | dq = np.dot(Jinv,vT(de)) + np.dot(Jprj,vT(phi)) 262 | dqmax = np.amax(np.absolute(dq)) 263 | for i in range(len(dq)) : 264 | if dqmax > dqlim[i] : 265 | dq[i] = dq[i]*(dqlim[i]/dqmax) 266 | return vT(dq) 267 | 268 | # Clamp given rotation angles to specified limits 269 | def clamp_rot(n,r,rmin,rmax): 270 | """ 271 | function [r] = clamp_rot(n,r,rmin,rmax) : returns r within [rmin,rmax] 272 | r = vector of n joint rotations (radians) 273 | rmin = vector of n joint rotation minimums (radians) 274 | rmax = vector of n joint rotation maximums (radians) 275 | """ 276 | global p360rad, n360rad, p180rad, n180rad 277 | 278 | # global dpr 279 | # print "clamp_rot: r= %8.2f, %8.2f %8.2f" % (r[0]*dpr, r[1]*dpr, r[2]*dpr) 280 | 281 | for i in range(n) : 282 | if abs(rmax[i]-rmin[i]) >= p360rad : 283 | if r[i] < n180rad : 284 | r[i] = p360rad + r[i] 285 | if r[i] > p180rad : 286 | r[i] = r[i] + n360rad 287 | if r[i] < rmin[i] : 288 | r[i] = rmin[i] 289 | elif r[i] > rmax[i] : 290 | r[i] = rmax[i] 291 | return r 292 | 293 | def time_to_goal(pt,vt,w,p,dq): 294 | """ 295 | function [tgo] = time_to_goal(pt,vt,w,p,dq) : returns estimated time for 296 | end-effector to reach the 297 | target goal 298 | pt = current position of target in world space 299 | vt = current velocity of target in world space 300 | w = set of n joint rotation direction vectors in world space 301 | p = set of n+1 position vectors (n joints + end-effector) in world space 302 | dq = vector of n joint rotation rates (radians/sec) 303 | """ 304 | n = len(dq) 305 | ie = len(p) - 1 306 | # absolute velocity of end effector 307 | ve = np.zeros(3) 308 | for i in range(n-1,-1,-1) : 309 | ve = ve + np.cross(dq[i]*w[i],(p[i+1]-p[i])) 310 | # closing vector between end-effector and target 311 | vecp = pt - p[ie] 312 | nrmp = la.norm(vecp) 313 | # estimated time to go 314 | if nrmp > 0.0 : 315 | vecn = vecp/nrmp 316 | tgo = nrmp/(np.dot(ve-vt,vecn)) 317 | if tgo < 0.0 : 318 | tgo = 0.0 319 | else : 320 | tgo = 0.0 321 | return tgo 322 | 323 | def closing_gain(ikm,ec,pt,vt): 324 | """ 325 | function [gain] = closing_gain(ikm,ec,pt,vt) : returns end effector closing gain 326 | ikm = IKmethod 327 | ec = end effector current position in world space 328 | pt = position of target in world space 329 | vt = velocity of target in world space 330 | """ 331 | de = ec - pt 332 | nde = la.norm(de) 333 | nvt = la.norm(vt) 334 | if (nde == 0.0) or (nvt == 0.0) : 335 | gain = 1.0 336 | else : 337 | # compute cosine of approach angle relative to target velocity vector 338 | aacos = np.dot(de/nde, vt/nvt) 339 | if aacos <= 0.0 : 340 | gain = 1.6 341 | elif aacos <= 0.7071 : 342 | gain = 1.2 343 | else : 344 | if ikm < 6 : gain = 1.0 # without null space control 345 | else : gain = 0.8 # with null space control 346 | return gain 347 | 348 | def avelT_wrt_jointm(pt,vt,w,p,dq,m): 349 | """ 350 | function [avel] = avelT_wrt_jointm(pt,vt,w,p,dq,m) : returns angular velocity of 351 | line-of-sight for the target 352 | with respect to joint m, 353 | assuming joint 1's position 354 | is fixed in world space. 355 | pt = current position of target in world space 356 | vt = current velocity of target in world space 357 | w = set of n joint rotation direction vectors in world space 358 | p = set of n+1 position vectors (n joints + end-effector) in world space 359 | dq = vector of n joint rotation rates (radians/sec) 360 | m = joint number of interest [1,n] 361 | """ 362 | try : 363 | assert m-1 in range(len(dq)), 'value of m-1 not in range(len(dq))' 364 | except AssertionError as err : 365 | ename = err.__class__.__name__ 366 | fname = 'avelT_wrt_jointm' 367 | print("%s in %s: %s" % (ename, fname, err) ) 368 | sys.exit() 369 | 370 | n = len(dq) # number of joints 371 | ie = len(p) - 1 # index of end effector position vector 372 | # absolute velocity of joint m 373 | vJ = np.zeros(3) 374 | for i in range(0,m-1,1) : 375 | vJ = vJ + np.cross(dq[i]*w[i],(p[i+1]-p[i])) 376 | # relative velocity of target wrt joint m 377 | vTJ = vt - vJ 378 | # direction vector to target from joint m 379 | vecp = pt - p[m-1] 380 | nrmp = la.norm(vecp) 381 | # line-of-sight angular velocity for target wrt to joint m 382 | avel = np.cross(vecp,vTJ)/(nrmp*nrmp) 383 | return avel 384 | 385 | def avelE_wrt_jointm(w,p,dq,m): 386 | """ 387 | function [avel] = avelE_wrt_jointm(w,p,dq,m) : returns angular velocity of 388 | line-of-sight for end-effector 389 | with respect to joint m, 390 | assuming joint 1's position 391 | is fixed in world space. 392 | w = set of n joint rotation direction vectors in world space 393 | p = set of n+1 position vectors (n joints + end-effector) in world space 394 | dq = vector of n joint rotation rates (radians/sec) 395 | m = joint number of interest [1,n] 396 | """ 397 | try : 398 | assert m-1 in range(len(dq)), 'value of m-1 not in range(len(dq))' 399 | except AssertionError as err : 400 | ename = err.__class__.__name__ 401 | fname = 'avelE_wrt_jointm' 402 | print("%s in %s: %s" % (ename, fname, err) ) 403 | sys.exit() 404 | 405 | n = len(dq) # number of joints 406 | ie = len(p) - 1 # index of end effector position vector 407 | # absolute velocity of end-effector 408 | vE = np.zeros(3) 409 | for i in range(n-1,-1,-1) : 410 | vE = vE + np.cross(dq[i]*w[i],(p[i+1]-p[i])) 411 | # absolute velocity of joint m 412 | vJ = np.zeros(3) 413 | for i in range(0,m-1,1) : 414 | vJ = vJ + np.cross(dq[i]*w[i],(p[i+1]-p[i])) 415 | # relative velocity of end-effector wrt joint m 416 | vEJ = vE - vJ 417 | # direction vector to end-effector from joint m 418 | vecp = p[ie] - p[m-1] 419 | nrmp = la.norm(vecp) 420 | # line-of-sight angular velocity for end-effector wrt joint m 421 | avel = np.cross(vecp,vEJ)/(nrmp*nrmp) 422 | return avel 423 | 424 | # Debug print for checking computed joint rotation angles. 425 | def angle_chk(q,u,p,et): 426 | """ 427 | function angle_chk(q,u,p,et) 428 | q = vector of n joint rotations (radians) 429 | u = set of n joint local rotation axis unit vectors 430 | p = set of n link joint position vectors 431 | et = target position vector 432 | """ 433 | global dpr 434 | 435 | print("* Angle Check:") 436 | n = len(q) 437 | k = 1 438 | a = q[k-1]*dpr 439 | print("joint %1d rotation angle from inertial x-axis = %12.6f" % (k,a) ) 440 | for i in range(1,n) : 441 | a = q[i]*dpr 442 | print("joint %1d rotation angle from joint %1d x-axis = %12.6f" % (i+1,i,a) ) 443 | asum = np.sum(q)*dpr 444 | print("angle to joint %1d x-axis from inertial x-axis = %12.6f" % (n,asum) ) 445 | 446 | ept = et - p[n-1] # vector from joint n to target 447 | npt = la.norm(ept) # distance from joint n to target 448 | ux = np.array([1.0,0.0,0.0]) 449 | uxw = vT(np.dot(rotation(np.sum(q),u[n-1]),vT(ux))) 450 | at3 = np.arccos(np.dot(ept,uxw)/npt)*dpr 451 | atI = np.arccos(np.dot(ept,ux)/npt)*dpr 452 | print("angle to target from joint %1d x-axis = %12.6f" % (n,at3) ) 453 | print("angle to target from inertial x-axis = %12.6f" % (atI) ) 454 | -------------------------------------------------------------------------------- /IK_Solver.py: -------------------------------------------------------------------------------- 1 | # File: IK_Solver.py 2 | # Auth: G. E. Deschaines 3 | # Date: 17 Apr 2015 4 | # Prog: Inverse Kinematics (IK) Solver 5 | # Desc: Applies user selected IK solving technique to determine and 6 | # plot the motion of a revolute jointed n-link chain in 2D or 7 | # 3D space. 8 | # 9 | # This Python program presents a 3D implementation of inverse kinematic 10 | # solutions to end effector movement for a n-link, revolute-joint chain 11 | # towards a target using cyclic coordinate descent (CCD) [1], Jacobian 12 | # transpose method (JTM) [3], pseudo-inverse method (PIM) [1,2,3] or 13 | # damped least squares (DLS) [3] as detailed in the listed references. 14 | # 15 | # The program source is comprised of the following Python files. 16 | # 17 | # IK_Solver.py - main routine and animation plot functions 18 | # IK_Solver_nlink.py - IK solver n-link chain definition function 19 | # IK_Solver_class.py - IK solver iteration and state data accessors 20 | # IK_Solver_funcs.py - IK solver math methods and support functions 21 | # 22 | # The IK solution technique is user selectable, and the program can be 23 | # configured to record the plotted link chain motion to a video file 24 | # for animation playback (requires ffmpeg or avconv). 25 | # 26 | # References: 27 | # 28 | # [1] Benjamin Kenwright, "Practical 01: Inverse Kinematics", September 29 | # 2014, School of Computer Science, Edinburgh Napier University, 30 | # United Kingdom, Physics-Based Animation practical web available at 31 | # http://games.soc.napier.ac.uk/study/pba_practicals/Practical%2001%20-%20Inverse%20Kinematics.pdf 32 | # 33 | # [2] Ben Kenwright, "Real-Time Character Inverse Kinematics using 34 | # the Gauss-Seidel Iterative Approximation Method", July 2012, 35 | # CONTENT 2012, The Fourth International Conference on Creative 36 | # Content Technologies, web available at 37 | # http://www.thinkmind.org/index.php?view=article&articleid=content_2012_4_10_60013 38 | # 39 | # [3] Samuel R. Buss, "Introduction to Inverse Kinematics with Jacobian 40 | # Transpose, Pseudoinverse and Damped Least Squares methods", October 41 | # 2009, Department of Mathematics, University of California, San 42 | # Diego, unpublished article web available at 43 | # http://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/iksurvey.pdf 44 | # 45 | # Disclaimer: 46 | # 47 | # See DISCLAIMER 48 | 49 | import sys 50 | import os 51 | import time 52 | 53 | from math import ceil 54 | 55 | import warnings 56 | warnings.simplefilter(action='ignore', category=FutureWarning) 57 | 58 | try: 59 | import numpy as np 60 | import scipy.linalg as la 61 | except ImportError: 62 | print("* Error: NumPy and SciPy packages required.") 63 | print(" Suggest installing the SciPy stack.") 64 | sys.exit() 65 | 66 | try: 67 | import matplotlib.pyplot as plt 68 | import matplotlib.animation as animation 69 | from matplotlib.lines import Line2D 70 | from mpl_toolkits.mplot3d.art3d import Line3D 71 | except ImportError: 72 | print("* Error: matplotlib package required.") 73 | print(" Suggest installing the SciPy stack.") 74 | sys.exit() 75 | 76 | try: 77 | from IK_Solver_nlink import * 78 | from IK_Solver_class import * 79 | except ImportError: 80 | print("* Error: IK_Solver_class.py required.") 81 | sys.exit() 82 | 83 | # Handle Python 2.x and 3.x raw_input. 84 | 85 | try: 86 | type(raw_input) 87 | except NameError: 88 | def raw_input(prompt): 89 | return input(prompt) 90 | 91 | # Animation plotting functions. 92 | 93 | def init2D(): 94 | """ 95 | init_func for 2D plotting 96 | """ 97 | global line1, line2, line3, line4, line5, time_text 98 | 99 | line1.set_data([], []) 100 | line2.set_data([], []) 101 | line3.set_data([], []) 102 | line4.set_data([], []) 103 | line5.set_data([], []) 104 | time_text.set_text('') 105 | 106 | # return artists in drawing order 107 | return line5, line1, line3, line4, line2, time_text 108 | 109 | def animate2D(n): 110 | """ 111 | animation function for 2D plotting 112 | """ 113 | global ik_solver, tdel 114 | global line1, line2, line3, line4, line5, time_text 115 | 116 | if n == 0 : 117 | ik_solver.set_target_pos( 3.0, 4.0, 0.0) 118 | ik_solver.set_target_vel(-0.1, 0.0, 0.0) 119 | 120 | ik_solver.step(tdel) 121 | if n == 0 : 122 | ik_solver.zero() 123 | 124 | (X0,Y0) = ik_solver.get_points_x0y0() 125 | (X,Y) = ik_solver.get_points_xy() 126 | (XT,YT) = ik_solver.get_target_xy() 127 | (XPT,YPT) = ik_solver.get_predtgt_xy() 128 | 129 | i = np.size(X0) - 1 130 | X0PT = np.array([X0[i],XPT[0]]) 131 | Y0PT = np.array([Y0[i],YPT[0]]) 132 | line1.set_data(X0,Y0) 133 | line2.set_data(X,Y) 134 | line3.set_data(XT,YT) 135 | line4.set_data(XPT,YPT) 136 | line5.set_data(X0PT,Y0PT) 137 | time_text.set_text('time = %.3f' % ik_solver.tsolve) 138 | 139 | # return artists in drawing order 140 | return line5, line1, line3, line4, line2, time_text 141 | 142 | def init3D(): 143 | """ 144 | init_func for 3D plotting 145 | """ 146 | global line1, line2, line3, line4, line5,\ 147 | line6, line7, line8, line9, time_text 148 | 149 | line1.set_data(np.array([]), np.array([])) 150 | line1.set_3d_properties(np.array([])) 151 | line2.set_data(np.array([]), np.array([])) 152 | line2.set_3d_properties(np.array([])) 153 | line3.set_data(np.array([]), np.array([])) 154 | line3.set_3d_properties(np.array([])) 155 | line4.set_data(np.array([]), np.array([])) 156 | line4.set_3d_properties(np.array([])) 157 | line5.set_data(np.array([]), np.array([])) 158 | line5.set_3d_properties(np.array([])) 159 | line6.set_data(np.array([]), np.array([])) 160 | line6.set_3d_properties(np.array([])) 161 | line7.set_data(np.array([]), np.array([])) 162 | line7.set_3d_properties(np.array([])) 163 | line8.set_data(np.array([]), np.array([])) 164 | line8.set_3d_properties(np.array([])) 165 | line9.set_data(np.array([]), np.array([])) 166 | line9.set_3d_properties(np.array([])) 167 | time_text.set_text('') 168 | 169 | # return artists in drawing order 170 | return line7, line8, line9, line4, line5,\ 171 | line6, line1, line3, line2, time_text 172 | 173 | def animate3D(n): 174 | """ 175 | animation function for 3D plotting 176 | """ 177 | global ik_solver, tdel 178 | global line1, line2, line3, line4, line5,\ 179 | line6, line7, line8, line9, time_text 180 | 181 | if n == 0 : 182 | ik_solver.set_target_pos( 3.0, 2.5, 2.0) 183 | ik_solver.set_target_vel( 0.0,-0.2, 0.0) 184 | 185 | ik_solver.step(tdel) 186 | if n == 0 : 187 | ik_solver.zero() 188 | 189 | (X0,Y0,Z0) = ik_solver.get_points_x0y0z0() 190 | (X,Y,Z) = ik_solver.get_points_xyz() 191 | (XT,YT,ZT) = ik_solver.get_target_xyz() 192 | (XE,YE,ZE) = ik_solver.get_endeff_xyz() 193 | (XPT,YPT,ZPT) = ik_solver.get_predtgt_xyz() 194 | 195 | line1.set_data(X0,Y0) 196 | line1.set_3d_properties(Z0) 197 | line2.set_data(X,Y) 198 | line2.set_3d_properties(Z) 199 | line3.set_data(XT,YT) 200 | line3.set_3d_properties(ZT) 201 | i = np.size(X0) - 1 202 | X0P = np.array([X0[i],X0[i],ax.get_xlim()[0]],dtype=object) 203 | Y0P = np.array([Y0[i],ax.get_ylim()[1],Y0[i]],dtype=object) 204 | Z0P = np.array([ax.get_zlim()[0],Z0[i],Z0[i]],dtype=object) 205 | line4.set_data(X0P,Y0P) 206 | line4.set_3d_properties(Z0P) 207 | XEP = np.array([XE,XE,ax.get_xlim()[0]],dtype=object) 208 | YEP = np.array([YE,ax.get_ylim()[1],YE],dtype=object) 209 | ZEP = np.array([ax.get_zlim()[0],ZE,ZE],dtype=object) 210 | line5.set_data(XEP,YEP) 211 | line5.set_3d_properties(ZEP) 212 | XTP = np.array([XPT,XPT,ax.get_xlim()[0]],dtype=object) 213 | YTP = np.array([YPT,ax.get_ylim()[1],YPT],dtype=object) 214 | ZTP = np.array([ax.get_zlim()[0],ZPT,ZPT],dtype=object) 215 | line6.set_data(XTP,YTP) 216 | line6.set_3d_properties(ZTP) 217 | X0TP0 = np.array([X0P[0],XTP[0]],dtype=object) 218 | Y0TP0 = np.array([Y0P[0],YTP[0]],dtype=object) 219 | Z0TP0 = np.array([Z0P[0],ZTP[0]],dtype=object) 220 | line7.set_data(X0TP0,Y0TP0) 221 | line7.set_3d_properties(Z0TP0) 222 | X0TP1 = np.array([X0P[1],XTP[1]],dtype=object) 223 | Y0TP1 = np.array([Y0P[1],YTP[1]],dtype=object) 224 | Z0TP1 = np.array([Z0P[1],ZTP[1]],dtype=object) 225 | line8.set_data(X0TP1,Y0TP1) 226 | line8.set_3d_properties(Z0TP1) 227 | X0TP2 = np.array([X0P[2],XTP[2]],dtype=object) 228 | Y0TP2 = np.array([Y0P[2],YTP[2]],dtype=object) 229 | Z0TP2 = np.array([Z0P[2],ZTP[2]],dtype=object) 230 | line9.set_data(X0TP2,Y0TP2) 231 | line9.set_3d_properties(Z0TP2) 232 | time_text.set_text('time = %.3f' % ik_solver.tsolve) 233 | 234 | # return artists in drawing order 235 | return line7, line8, line9, line4, line5,\ 236 | line6, line1, line3, line2, time_text 237 | 238 | # IK_Solver main. 239 | 240 | if __name__ == '__main__': 241 | 242 | # Processing selectors 243 | 244 | IKmethod = 0 # selected IK solver method 245 | Plot3D = 0 # plot in 3D flag 246 | Record = 0 # record movie flag 247 | 248 | # Check program arguments for IK method, Plot3D and Record values. 249 | 250 | sval = '0' 251 | if ( len(sys.argv) > 1 ) : 252 | sval = sys.argv[1] 253 | if ( len(sys.argv) > 2 ) : 254 | Plot3D = eval(sys.argv[2]) 255 | if ( len(sys.argv) > 3 ) : 256 | Record = eval(sys.argv[3]) 257 | 258 | # Request IK method selection (if not already given). 259 | 260 | prompt = 'Select IK Solver (1=CCD,2=JTM,3=PIM2,4=PIM3,5=DLS)? ' 261 | while IKmethod == 0 : 262 | if sval == '0' : 263 | sval = raw_input(prompt) 264 | if sval == '1' : 265 | IKmethod = UseCCD 266 | title = 'IK_Solver - Cyclic Coordinate Descent' 267 | elif sval == '2' : 268 | IKmethod = UseJTM 269 | title = 'IK_Solver - Using Jacobian Transpose Method' 270 | elif sval == '3' : 271 | IKmethod = UsePIM2 272 | title = 'IK_Solver - Using Pseudo-Inverse Method [ref 2]' 273 | elif sval == '4' : 274 | IKmethod = UsePIM3 275 | title = 'IK_Solver - Using Pseudo-Inverse Method [ref 3]' 276 | elif sval == '5' : 277 | IKmethod = UseDLS 278 | title = 'IK_Solver - Using Damped Least Squares [ref 3]' 279 | elif sval == '6' : 280 | IKmethod = UsePIM3dH 281 | title = 'IK_Solver - Using Pseudo-Inverse Method with null space control [ref 3]' 282 | elif sval == '7' : 283 | IKmethod = UseDLSdH 284 | title = 'IK_Solver - Using Damped Least Squares with null space control [ref 3]' 285 | else : 286 | sval = '0' 287 | IKmethod = 0 288 | 289 | # Define link chain structure and parameters. 290 | 291 | (a, u, q0, qmin, qmax, dqlim) = get_nlink_chain(Plot3D) 292 | 293 | # Instantiate an IK_Solver. 294 | 295 | ik_solver = IK_Solver(Plot3D,IKmethod,a,u,q0,qmin,qmax,dqlim) 296 | 297 | # If recording animation, assign an animation writer. 298 | 299 | Writer = None 300 | if Record == 1 : 301 | try: 302 | Writer = animation.writers['ffmpeg'] 303 | except KeyError: 304 | try: 305 | Writer = animation.writers['avconv'] 306 | except KeyError: 307 | print("Cannot record animation, no animation writers available!") 308 | print("Try installing ffmpeg or avconv.") 309 | Record = 0 310 | 311 | # Instantiate a matplotlib pyplot figure. 312 | 313 | if Record == 0 : 314 | fig = plt.figure(figsize=(8,6), dpi=80) 315 | else : 316 | fig = plt.figure(figsize=(6,4), dpi=80) 317 | 318 | # Specify plotting objects and parameters. 319 | 320 | if Plot3D == 0 : 321 | ax = fig.add_subplot(111, autoscale_on=False) 322 | line1 = Line2D([],[],color='g',ls='-',lw=1.0, # link chain at start 323 | marker='o',mew=1.0,mec='k',mfc='g') 324 | line2 = Line2D([],[],color='b',ls='-',lw=2.0, # link chain in motion 325 | marker='o',mew=1.0,mec='k',mfc='b') 326 | line3 = Line2D([],[],color='r',ls=' ',lw=2.0, # current target location 327 | marker='x',mew=2.0,mec='r',mfc='r') 328 | line4 = Line2D([],[],color='m',ls=' ',lw=2.0, # predicted target location 329 | marker='x',mew=2.0,mec='m',mfc='m') 330 | line5 = Line2D([],[],color='k',ls=':',lw=1.0, # eff to predicted tgt line 331 | marker=' ') 332 | time_text = ax.text(0.02, 0.95, '', transform=ax.transAxes) 333 | ax.set_title(title) 334 | ax.set_xlabel('X') 335 | ax.set_ylabel('Y') 336 | ax.set_xlim(-6,6) 337 | ax.set_ylim(-6,6) 338 | ax.set_aspect('equal') 339 | ax.grid() 340 | ax.add_line(line1) 341 | ax.add_line(line2) 342 | ax.add_line(line3) 343 | ax.add_line(line4) 344 | ax.add_line(line5) 345 | fig.canvas.draw() 346 | else : 347 | ax = fig.add_subplot(111, projection='3d') 348 | line1 = Line3D([],[],[],color='g',ls='-',lw=1.0, # link chain start in 3D 349 | marker='o',mew=1.0,mec='k',mfc='g') 350 | line2 = Line3D([],[],[],color='b',ls='-',lw=2.0, # link chain motion in 3D 351 | marker='o',mew=1.0,mec='k',mfc='b') 352 | line3 = Line3D([],[],[],color='r',ls=' ',lw=2.0, # target in 3D space 353 | marker='x',mew=2.0,mec='r',mfc='r') 354 | line4 = Line3D([],[],[],color='g',ls=' ',lw=1.0, # eff start in XYZ planes 355 | marker='o',mew=1.0,mec='g',mfc='w') 356 | line5 = Line3D([],[],[],color='b',ls=' ',lw=1.0, # eff motion in XYZ planes 357 | marker='o',mew=1.0,mec='b',mfc='w') 358 | line6 = Line3D([],[],[],color='m',ls=' ',lw=1.0, # predicted target in XYZ planes 359 | marker='x',mew=1.0,mec='m',mfc='m') 360 | line7 = Line3D([],[],[],color='k',ls=':',lw=1.0, # eff to pred. tgt line in XY plane 361 | marker=' ') 362 | line8 = Line3D([],[],[],color='k',ls=':',lw=1.0, # eff to pred. tgt line in XZ plane 363 | marker=' ') 364 | line9 = Line3D([],[],[],color='k',ls=':',lw=1.0, # eff to pred. tgt line in YZ plane 365 | marker=' ') 366 | time_text = ax.text3D(-6.0, -7.0, 6.5, '') 367 | ax.set_title(title) 368 | ax.set_xlabel('X') 369 | ax.set_ylabel('Y') 370 | ax.set_zlabel('Z') 371 | ax.set_xlim3d([-5,5]) 372 | ax.set_ylim3d([-5,5]) 373 | ax.set_zlim3d([ 0,5]) 374 | #ax.set_aspect('equal') 375 | ax.grid() 376 | ax.add_line(line1) 377 | ax.add_line(line2) 378 | ax.add_line(line3) 379 | ax.add_line(line4) 380 | ax.add_line(line5) 381 | ax.add_line(line6) 382 | ax.add_line(line7) 383 | ax.add_line(line8) 384 | ax.add_line(line9) 385 | fig.canvas.draw() 386 | 387 | # Determine animation interval time. 388 | 389 | FPS = 10 390 | tdel = 1.0/FPS 391 | tdel_msec = 1000.0*tdel 392 | try: 393 | tmsec0 = 1000.0*time.clock() 394 | except: 395 | tmsec0 = 1000.0*time.process_time() 396 | if Plot3D == 0 : 397 | animate2D(0) 398 | else : 399 | animate3D(0) 400 | try: 401 | tmsec1 = 1000.0*time.clock() 402 | except: 403 | tmsec1 = 1000.0*time.process_time() 404 | tstep = tmsec1 - tmsec0 405 | interval = ceil(tmsec1-tmsec0) # allows faster than real-time 406 | #interval = tdel_msec - tstep # approximates real-time 407 | """ 408 | print("tdel_msec = %8.3f" % tdel_msec) 409 | print("t1 - t0 = %8.3f" % tstep) 410 | print("interval = %8.3f" % interval) 411 | """ 412 | 413 | # Assign mouse button press handler. 414 | 415 | cid = fig.canvas.mpl_connect('button_press_event', ik_solver.onclick) 416 | 417 | # Specify animation parameters and assign functions. 418 | 419 | nframes = None 420 | cache = False 421 | blit = False # blitting not working well with Matplotlib v1.5.1+ 422 | if Record == 1 : 423 | nframes = 14*FPS 424 | cache = True 425 | blit = False 426 | 427 | if Plot3D == 0 : 428 | anim = animation.FuncAnimation(fig, animate2D, init_func=init2D, 429 | frames=nframes, blit=blit, 430 | cache_frame_data=cache, 431 | interval=interval, repeat=False) 432 | else : 433 | anim = animation.FuncAnimation(fig, animate3D, init_func=init3D, 434 | frames=nframes, blit=blit, 435 | cache_frame_data=cache, 436 | interval=interval, repeat=False) 437 | 438 | # Begin. 439 | 440 | if Record == 0 or Writer is None: 441 | if Plot3D == 0 : 442 | print("Move cursor into plot region and press middle mouse button") 443 | print("to initiate IK solver for selected target location.") 444 | else : 445 | print("With cursor in the plot region, press middle mouse button") 446 | print("to initiate IK solver for randomly positioned target.") 447 | plt.show() 448 | else : 449 | plt.ioff() 450 | print("Creating animation video; presentation will begin shortly ...") 451 | writer = Writer(fps=FPS, codec='libx264', 452 | metadata=dict(artist='IK_Solver'), 453 | bitrate=-1) 454 | print("Performing IK solver iteration...") 455 | if Plot3D == 0 : 456 | filename = 'IK_Solver_' + str(IKmethod) + '_2D.mp4' 457 | else: 458 | filename = 'IK_Solver_' + str(IKmethod) + '_3D.mp4' 459 | anim.save(filename, writer=writer) 460 | print("Animation saved in file %s" % filename) 461 | plt.show() 462 | 463 | # Terminate and exit. 464 | 465 | del ik_solver 466 | -------------------------------------------------------------------------------- /IK_Solver_class.py: -------------------------------------------------------------------------------- 1 | # File: IK_Solver_class.py 2 | # Auth: G. E. Deschaines 3 | # Date: 27 Apr 2015 4 | # Prog: Inverse Kinematics (IK) Solver class 5 | # Desc: Applies user selected IK solving technique to determine and 6 | # plot the motion of a jointed n-link chain in 2D or 3D space. 7 | # 8 | # This file provides the following IK_Solver class methods intended 9 | # for public use 10 | # 11 | # __init__ - IK_Solver class object constructor 12 | # step - performs IK solver iteration time step 13 | # zero - sets IK solver state to initial conditions 14 | # set_target_pos - sets target position [x,y,z] coordinates 15 | # set_target_vel - sets target velocity [x,y,z] components 16 | # get_points_x0y0 - gets initial link coordinates for 2D plots 17 | # get_points_x0y0z0 - gets initial link coordinates for 3D plots 18 | # get_points_xy - gets current link coordinates for 2D plots 19 | # get_points_xyz - gets current link coordinates for 3D plots 20 | # get_target_xy - gets current target coordinates for 2D plots 21 | # get_target_xyz - gets current target coordinates for 3D plots 22 | # get_predtgt_xy - gets predicted target coordinates for 2D plots 23 | # get_predtgt_xyz - gets predicted target coordinates for 3D plots 24 | # get_endeff_xyz - gets current end effector coordinates for 3D plots 25 | # get_endeff_rot - gets current end effector rotation about its z-axis 26 | # get_endeff_ypr - gets current end effector yaw,pitch,roll rotations 27 | # get_joints_rot - gets current joints rotation angle 28 | # mouse_click - sets target position based on mouse click 29 | # onclick - mouse button press handler 30 | # 31 | # and these methods intended only for private use. 32 | # 33 | # done - applies IK solver completion criteria 34 | # null_space_control - computes null space control vector 35 | # set_points_x0y0z0 - sets starting link coordinates for plotting 36 | # 37 | # Disclaimer: 38 | # 39 | # See DISCLAIMER 40 | 41 | import sys 42 | import random 43 | 44 | from math import atan,cos,sin,floor 45 | 46 | try: 47 | import numpy as np 48 | import scipy.linalg as la 49 | except ImportError: 50 | print("* Error: NumPy and SciPy packages required.") 51 | print(" Suggest installing the SciPy stack.") 52 | sys.exit() 53 | 54 | try: 55 | from IK_Solver_funcs import * 56 | except ImportError: 57 | print("* Error: IK_Solver_funcs.py required.") 58 | sys.exit() 59 | 60 | UseCCD = 1 # use cyclic coordinate descent (CCD) from ref [1] 61 | UseJTM = 2 # use jacobian transpose method from ref [1,2] 62 | UsePIM2 = 3 # use pseudo-inverse method from ref [2] 63 | UsePIM3 = 4 # use pseudo-inverse method from ref [3] 64 | UseDLS = 5 # use damped least squares from ref [3] 65 | UsePIM3dH = 6 # use pseudo-inverse method from ref [3] with null space control 66 | UseDLSdH = 7 # use damped least squares from ref [3] with null space control 67 | 68 | class IK_Solver: 69 | 70 | def __init__(self,p3d,ik,a,u,q0,qmin,qmax,dqlim): 71 | """ 72 | IK_Solver class - object constructor 73 | """ 74 | self.Plot3D = p3d # plot mode (0=2D, 1=3D) 75 | self.IKmethod = ik # selected IK solver method 76 | self.a = a # set of link joint attachment location vectors 77 | self.u = u # set of joint rotation axis unit vectors 78 | self.q0 = q0 # vector of initial joint rotations (radians) 79 | self.qmin = qmin # vector of joint minimum rotations (radians) 80 | self.qmax = qmax # vector of joint maximum rotations (radians) 81 | self.dqlim = dqlim # joints delta rotation limit (radians) 82 | 83 | self.na = len(self.a) # number of link body attachment points 84 | self.ia = self.na - 1 # index of end effector attachment point 85 | self.nq = len(self.q0) # number of link chain joints 86 | self.s = np.zeros(self.nq) # create array of link sizes 87 | for i in range(0,self.na-1): 88 | self.s[i] = la.norm(a[i+1]) 89 | self.efd = self.s[-1] # size of end effector link 90 | 91 | # generalizations used in distance checks of iteration done function; 92 | # assumes joint rotation limits do not prevent a full linear extension. 93 | if p3d == 0: 94 | self.dxy = np.sum(self.s[0:]) # length of fully extended chain in xy plane 95 | self.dz = 0.0 # length of fully extended chain in z direction 96 | else: 97 | # following assumes first link strictly in the z direction 98 | self.dxy = np.sum(self.s[1:]) # length of fully extended chain in xy plane 99 | self.dz = np.sum(self.s[0:]) # length of fully extended chain in z direction 100 | 101 | self.q = self.q0 # vector of current joint rotations 102 | self.dq = np.zeros(self.nq) # vector of current joint delta rotations 103 | 104 | # transform joint positions/rotations from link body coordinates 105 | # to world space 106 | (self.p,self.w) = transform(self.na,self.q,self.u,self.a) 107 | 108 | # get end effector current position vector 109 | self.ec = self.p[self.ia] 110 | 111 | # specify end effector target position and velocity 112 | if self.Plot3D == 0 : 113 | self.et0 = np.array([ 3.0, 4.0, 0.0]) # position vector 114 | self.vt0 = np.array([-0.1, 0.0, 0.0]) # velocity vector 115 | else : 116 | self.et0 = np.array([ 3.0, 2.5, 2.0]) # position vector 117 | self.vt0 = np.array([ 0.0,-0.2, 0.0]) # velocity vector 118 | self.et = self.et0 # end effector target position in world space 119 | self.vt = self.vt0 # velocity of target in world space 120 | self.pt = self.et0 # predicted target position in world space 121 | 122 | self.h = 0.04 # IK iteration step size 123 | self.ilim = int(30.0/self.h) # IK iteration limit for 30 seconds 124 | self.sdel = 0.001 # PIM singularity damping constant from ref [2] 125 | self.sfac = 0.01 # PIM singularity threshold factor from ref [3] 126 | self.slam = 1.1 # DLS singularity damping factor from ref [3] 127 | self.dH = np.zeros(self.nq) # null space control vector 128 | self.derr = 0.04*self.efd # allowable effector to target distance error 129 | self.perr = atan(self.derr/self.efd) # allowable effector to target xy pointing error 130 | self.tsolve = 0.0 # time to solve 131 | self.tgo = self.h # time to goal 132 | self.ni = 0 # number of iterations 133 | self.lastni = 0 # save of last ni on done and print 134 | self.button = 0 # mouse button pressed indicator 135 | self.X0 = np.zeros(len(self.p)) # link chain initial x coordinates 136 | self.Y0 = np.zeros(len(self.p)) # link chain initial y coordinates 137 | self.Z0 = np.zeros(len(self.p)) # link chain initial y coordinates 138 | self.set_points_x0y0z0() 139 | 140 | # compute closest horizontal approach distance of target to link chain base 141 | etp = self.et - self.p[0] # vector from link chain base to target 142 | nvt = la.norm(self.vt[0:2]) # norm of horizontal component of target velocity 143 | if nvt > 0.0 : 144 | uvt = self.vt[0:2]/nvt 145 | else : 146 | uvt = np.array([0.0, 0.0]) 147 | self.dxymin = la.norm(etp[0:2] - np.dot(etp[0:2], uvt)*uvt) 148 | 149 | # set random number generator seed 150 | random.seed(987654321) 151 | 152 | def zero(self): 153 | """ 154 | IK_Solver class - zero object state function 155 | """ 156 | self.q = self.q0 157 | self.dq = np.zeros(self.nq) 158 | (self.p,self.w) = transform(self.na,self.q,self.u,self.a) 159 | self.ec = self.p[self.ia] 160 | self.et = self.et0 161 | self.vt = self.vt0 162 | self.pt = self.et0 163 | self.dq = np.zeros(self.nq) 164 | self.dH = np.zeros(self.nq) 165 | self.tsolve = 0.0 166 | self.tgo = self.h 167 | self.ni = 0 168 | self.lastni = 0 169 | self.button = 0 170 | self.set_points_x0y0z0() 171 | 172 | def null_space_control(self): 173 | """ 174 | IK solver class - null space control vector computation function 175 | 176 | NOTE: This function is tailored specifically to hardcoded n-link 177 | chains defined in IK_Solver_nlink.py 178 | """ 179 | if self.Plot3D == 0 : 180 | ''' 181 | print("dq = %8.5f %8.5f %8.5f %8.5f" % \ 182 | (self.dq[0],self.dq[1],self.dq[2],self.dq[4]) ) 183 | ''' 184 | avT1 = avelT_wrt_jointm(self.et,self.vt,self.w,self.p,self.dq,1) 185 | avT2 = avelT_wrt_jointm(self.et,self.vt,self.w,self.p,self.dq,2) 186 | avT3 = avelT_wrt_jointm(self.et,self.vt,self.w,self.p,self.dq,3) 187 | avT5 = avelT_wrt_jointm(self.et,self.vt,self.w,self.p,self.dq,5) 188 | avT = np.array([avT1[2], avT2[2], avT3[2], 0.0, avT5[2]]) 189 | ''' 190 | print("avT = %8.5f %8.5f %8.5f %8.5f" % \ 191 | (avT[0],avT[1],avT[2],avT[4]) ) 192 | ''' 193 | avE1 = avelE_wrt_jointm(self.w,self.p,self.dq,1) 194 | avE2 = avelE_wrt_jointm(self.w,self.p,self.dq,2) 195 | avE3 = avelE_wrt_jointm(self.w,self.p,self.dq,3) 196 | avE5 = avelE_wrt_jointm(self.w,self.p,self.dq,5) 197 | avE = np.array([avE1[2], avE2[2], avE3[2], 0.0, avE5[2]]) 198 | ''' 199 | print("avE = %8.5f %8.5f %8.5f %8.5f" % \ 200 | (avE[0],avE[1],avE[2],avE[4]) ) 201 | ''' 202 | self.dH[0] = avE[0] 203 | self.dH[1] = avE[1] 204 | self.dH[2] = avE[2] 205 | #self.dH[4] = avE[4] 206 | 207 | phiZ = self.get_endeff_rot() 208 | ''' 209 | print("phiZ = %8.5f" % (phiZ*dpr)) 210 | ''' 211 | self.dH[self.nq-1] = phiZ 212 | else : 213 | ''' 214 | print("dq = %8.5f %8.5f %8.5f" % \ 215 | (self.dq[0],self.dq[1],self.dq[2]) ) 216 | ''' 217 | avT1 = avelT_wrt_jointm(self.et,self.vt,self.w,self.p,self.dq,1) 218 | avT2 = avelT_wrt_jointm(self.et,self.vt,self.w,self.p,self.dq,2) 219 | avT3 = avelT_wrt_jointm(self.et,self.vt,self.w,self.p,self.dq,3) 220 | avT = np.array([avT1[2], avT2[1], avT3[1], 0.0, 0.0]) 221 | ''' 222 | print("avT = %8.5f %8.5f %8.5f" % (avT[0],avT[1],avT[2]) ) 223 | ''' 224 | avE1 = avelE_wrt_jointm(self.w,self.p,self.dq,1) 225 | avE2 = avelE_wrt_jointm(self.w,self.p,self.dq,2) 226 | avE3 = avelE_wrt_jointm(self.w,self.p,self.dq,3) 227 | avE = np.array([avE1[2], avE2[1], avE3[1], 0.0, 0.0]) 228 | ''' 229 | print("avE = %8.5f %8.5f %8.5f" % (avE[0],avE[1],avE[2]) ) 230 | ''' 231 | self.dH[0] = avE[0] 232 | self.dH[1] = avE[1] 233 | self.dH[2] = avE[2] 234 | 235 | (psi,theta,phi) = self.get_endeff_ypr() 236 | ''' 237 | print("psi,theta,phi = %8.5f, %8.5f, %8.5f" % \ 238 | (psi*dpr,theta*dpr,phi*dpr) ) 239 | ''' 240 | self.dH[self.nq-2] = -theta 241 | self.dH[self.nq-1] = -psi 242 | 243 | def done(self,ni): 244 | """ 245 | IK Solver class - iteration done check function 246 | """ 247 | if ni == self.ilim : return True 248 | 249 | etp = self.et-self.p[0] # vector from link chain base to target 250 | ecp = self.ec-self.p[0] # vector from link chain base to effector 251 | detxy = la.norm(etp[0:2]) # distance from base to target in xy plane 252 | detz = abs(etp[2]) # distance from base to target in z direction 253 | decxy = la.norm(ecp[0:2]) # distance from base to effector in xy plane 254 | dotxy = np.dot(ecp[0:2],etp[0:2])/detxy # portion of ecp along etp in xy plane 255 | 256 | if ((detxy - self.dxy) > self.derr) or ((detz - self.dz) > self.derr): 257 | # target currently beyond reach of effector; but is it 258 | # outside allowable distance or xy pointing error? 259 | if (abs(decxy - self.dxy) > self.derr) or \ 260 | (np.arccos(dotxy/decxy) > self.perr) : 261 | return False 262 | # could end-effector reach target at its closest horizontal approach? 263 | if (self.dxy > self.dxymin) : 264 | return False 265 | print("Encountered done condition 1.") 266 | else : 267 | # target currently not beyond reach of effector; but is it 268 | # outside allowable distance? 269 | if (la.norm(self.et - self.ec) > self.derr) : 270 | return False 271 | print("Encountered done condition 2.") 272 | return True 273 | 274 | def step(self,tdel): 275 | """ 276 | IK solver class - iteration time step function 277 | """ 278 | if ( self.button == -1 ) : return 279 | 280 | icnt = max(floor(tdel/self.h),1) 281 | 282 | while icnt > 0 : 283 | if self.done(self.ni) : 284 | if self.lastni != self.ni: 285 | print("Iteration time (sec) = %8.3f" % (self.tsolve) ) 286 | error = la.norm(self.ec - self.et) 287 | print("Effector position error = %8.4f" % (error) ) 288 | if self.Plot3D == 0 : 289 | theta = self.get_endeff_rot() 290 | print("Effector rotation angle = %8.3f " % (theta*dpr) ) 291 | else : 292 | (psi,theta,phi) = self.get_endeff_ypr() 293 | print("Effector yaw,pitch,roll = %8.3f, %8.3f, %8.3f" % \ 294 | (psi*dpr, theta*dpr, phi*dpr) ) 295 | self.lastni = self.ni # prevent done reprint of last IK solution 296 | self.button = -1 297 | icnt = 0 298 | else : 299 | # Calculate IK solution for dq 300 | if self.IKmethod == UseCCD : 301 | # predicted target position at tgo relative to link 302 | gain = closing_gain(self.IKmethod, self.ec, self.et, self.vt) 303 | ept = self.et + gain*self.tgo*self.vt 304 | self.dq = np.zeros(self.nq) 305 | for i in range(self.nq-1,-1,-1) : 306 | pt = ept - self.p[i] 307 | npt = la.norm(pt) 308 | pc = self.ec - self.p[i] 309 | npc = la.norm(pc) 310 | ut = np.cross(np.cross(self.w[i],pc/npc),\ 311 | np.cross(self.w[i],pt/npt)) 312 | if la.norm(ut) > 0.0 : 313 | ut = ut/la.norm(ut) 314 | dq = np.arccos(np.dot(pt,vT(pc/npc))/npt) 315 | dqmax = np.amin([dq,self.dqlim[i]]) 316 | self.dq[i] = dqmax*(np.dot(ut,vT(self.w[i]))) 317 | else : 318 | self.dq[i] = 0.0 319 | else : 320 | # error from effector to predicted target position at tgo 321 | gain = closing_gain(self.IKmethod, self.ec, self.et, self.vt) 322 | de = self.et + gain*self.tgo*self.vt - self.ec 323 | #de = self.et - self.ec 324 | #Jt = jacobian(self.nq,self.w,self.p,self.et+self.tgo*self.vt) 325 | Jc = jacobian(self.nq,self.w,self.p,self.ec) 326 | J = Jc 327 | if self.IKmethod == UseJTM : 328 | self.dq = ik_jtm(J,de,self.dqlim) 329 | elif self.IKmethod == UsePIM2 : 330 | self.dq = ik_pim2(J,de,self.sdel,self.dqlim) 331 | elif self.IKmethod == UsePIM3 : 332 | self.dq = ik_pim3(J,de,self.sfac,self.dqlim,self.dH) 333 | elif self.IKmethod == UseDLS : 334 | self.dq = ik_dls(J,de,self.slam,self.dqlim,self.dH) 335 | elif self.IKmethod == UsePIM3dH : 336 | # Use dH to coerce end-effector orientation 337 | self.null_space_control() 338 | self.dq = ik_pim3(J,de,self.sfac,self.dqlim,self.dH) 339 | elif self.IKmethod == UseDLSdH : 340 | # Use dH to coerce end-effector orientation 341 | self.null_space_control() 342 | self.dq = ik_dls(J,de,self.slam,self.dqlim,self.dH) 343 | # Update link chain angles and positions 344 | self.q = self.q + self.h*self.dq 345 | self.q = clamp_rot(self.nq,self.q,self.qmin,self.qmax) 346 | (self.p,self.w) = transform(self.na,self.q,self.u,self.a) 347 | ##angle_chk(self.q,self.u,self.p,self.et) 348 | # Update end effector current and target positions 349 | self.ec = self.p[self.ia] 350 | self.et = self.et + self.h*self.vt 351 | self.tgo = time_to_goal(self.et,self.vt,self.w,self.p,self.dq) 352 | gain = closing_gain(self.IKmethod, self.ec, self.et, self.vt) 353 | self.pt = self.et + gain*self.tgo*self.vt 354 | # Increment iteration counters and time 355 | self.lastni = self.ni 356 | self.ni = self.ni + 1 357 | self.tsolve = self.tsolve + self.h 358 | icnt = icnt - 1 359 | 360 | def set_points_x0y0z0(self): 361 | """ 362 | IK Solver - set initial position (X,Y,Z) coordinates 363 | """ 364 | n = len(self.p) 365 | for i in range(n) : 366 | self.X0[i] = self.p[i][0] 367 | self.Y0[i] = self.p[i][1] 368 | self.Z0[i] = self.p[i][2] 369 | 370 | def set_target_pos(self, x, y, z): 371 | """ 372 | IK Solver - set target position (X,Y,Z) coordinates 373 | """ 374 | self.et[0] = x 375 | self.et[1] = y 376 | self.et[2] = z 377 | 378 | def set_target_vel(self, vx, vy, vz): 379 | """ 380 | IK Solver - set target velocity (X,Y,Z) components 381 | """ 382 | self.vt[0] = vx 383 | self.vt[1] = vy 384 | self.vt[2] = vz 385 | 386 | def get_points_x0y0(self): 387 | """ 388 | IK Solver - get initial position (X,Y) coordinates 389 | """ 390 | n = len(self.p) 391 | X0 = np.zeros(n) 392 | Y0 = np.zeros(n) 393 | for i in range(n) : 394 | X0[i] = self.X0[i] 395 | Y0[i] = self.Y0[i] 396 | return (X0,Y0) 397 | 398 | def get_points_x0y0z0(self): 399 | """ 400 | IK Solver - get initial position (X,Y,Z) coordinates 401 | """ 402 | n = len(self.p) 403 | X0 = np.zeros(n) 404 | Y0 = np.zeros(n) 405 | Z0 = np.zeros(n) 406 | for i in range(n) : 407 | X0[i] = self.X0[i] 408 | Y0[i] = self.Y0[i] 409 | Z0[i] = self.Z0[i] 410 | return (X0,Y0,Z0) 411 | 412 | def get_points_xy(self): 413 | """ 414 | IK Solver - get current position (X,Y) coordinates 415 | """ 416 | n = len(self.p) 417 | X = np.zeros(n) 418 | Y = np.zeros(n) 419 | for i in range(n) : 420 | X[i] = self.p[i][0] 421 | Y[i] = self.p[i][1] 422 | return (X,Y) 423 | 424 | def get_points_xyz(self): 425 | """ 426 | IK Solver - get current position (X,Y,Z) coordinates 427 | """ 428 | n = len(self.p) 429 | X = np.zeros(n) 430 | Y = np.zeros(n) 431 | Z = np.zeros(n) 432 | for i in range(n) : 433 | X[i] = self.p[i][0] 434 | Y[i] = self.p[i][1] 435 | Z[i] = self.p[i][2] 436 | return (X,Y,Z) 437 | 438 | def get_target_xy(self): 439 | """ 440 | IK Solver - get current target (X,Y) coordinates 441 | """ 442 | X = np.array([self.et[0]]) 443 | Y = np.array([self.et[1]]) 444 | return (X,Y) 445 | 446 | def get_target_xyz(self): 447 | """ 448 | IK Solver - get current target (X,Y,Z) coordinates 449 | """ 450 | X = np.array([self.et[0]]) 451 | Y = np.array([self.et[1]]) 452 | Z = np.array([self.et[2]]) 453 | return (X,Y,Z) 454 | 455 | def get_predtgt_xy(self): 456 | """ 457 | IK Solver - get predicted target (X,Y) coordinates 458 | """ 459 | X = np.array([self.pt[0]]) 460 | Y = np.array([self.pt[1]]) 461 | return (X, Y) 462 | 463 | def get_predtgt_xyz(self): 464 | """ 465 | IK Solver - get predicted target (X,Y,Z) coordinates 466 | """ 467 | X = np.array([self.pt[0]]) 468 | Y = np.array([self.pt[1]]) 469 | Z = np.array([self.pt[2]]) 470 | return (X, Y, Z) 471 | 472 | def get_endeff_xyz(self): 473 | """ 474 | IK Solver - get current end effector (X,Y,Z) coordinates 475 | """ 476 | X = np.array([self.ec[0]]) 477 | Y = np.array([self.ec[1]]) 478 | Z = np.array([self.ec[2]]) 479 | return (X,Y,Z) 480 | 481 | def get_endeff_rot(self): 482 | """ 483 | IK Solver - get current end effector rotation angle 484 | in radians for rotation about the last 485 | link segment's z-axis. 486 | """ 487 | # Rotation matrix from world space to last joint local space 488 | R = np.eye(3,dtype=np.float64) 489 | for i in range(self.nq) : 490 | Ri = np.transpose(rotation(self.q[i],self.u[i])) 491 | R = np.dot(Ri,R) 492 | # World space X-axis in last joint local space 493 | vecX = vT(np.dot(R,vT([1.0,0.0,0.0]))) 494 | # Angle between last link segment and X-axis 495 | phiZ = np.arctan2(vecX[1],vecX[0]) 496 | return phiZ 497 | 498 | def get_endeff_ypr(self): 499 | """ 500 | IK Solver - get current end effector yaw, pitch and roll 501 | angles in radians for Euler rotations about 502 | the last link segment's zyx axes respectively. 503 | Assumes the pitch angle never reaches +/- 90 504 | degrees. 505 | """ 506 | # Rotation matrix from world space to last joint local space 507 | R = np.eye(3,dtype=np.float64) 508 | for i in range(self.nq) : 509 | Ri = np.transpose(rotation(self.q[i],self.u[i])) 510 | R = np.dot(Ri,R) 511 | # Euler rotation angles of last link segment 512 | yaw = np.arctan2(R[0,1],R[0,0]) 513 | pitch = -np.arcsin(R[0,2]) 514 | roll = np.arctan2(R[1,2],R[2,2]) 515 | return (yaw,pitch,roll) 516 | 517 | def get_joints_rot(self): 518 | """ 519 | IK Solver - get current joints rotation angle in radians 520 | """ 521 | n = len(self.q) 522 | Q = np.zeros(n) 523 | for i in range(n) : 524 | Q[i] = self.q[i] 525 | return (Q) 526 | 527 | def mouse_click(self, b, x, y): 528 | """ 529 | IK_Solver - store button and (x,y) states for mouse click 530 | """ 531 | self.button = b 532 | if self.button == 2 : 533 | if self.Plot3D == 0 : 534 | self.et0[0] = x 535 | self.et0[1] = y 536 | else : 537 | theta = 2*pi*random.random() 538 | r = random.uniform(2,4) 539 | self.et0[0] = r*cos(theta) 540 | self.et0[1] = r*sin(theta) 541 | self.et0[2] = random.uniform(1,3) 542 | self.et = self.et0 543 | self.pt = self.et0 544 | self.dq = np.zeros(self.nq) 545 | self.dH = np.zeros(self.nq) 546 | self.tsolve = 0.0 547 | self.tgo = self.h 548 | self.ni = 0 549 | self.set_points_x0y0z0() 550 | 551 | def onclick(self, event): 552 | """ 553 | IK Solver - mouse button pressed handler 554 | """ 555 | b = event.button 556 | x = event.xdata 557 | y = event.ydata 558 | self.mouse_click(b, x, y) 559 | -------------------------------------------------------------------------------- /IK_Solver_client.py: -------------------------------------------------------------------------------- 1 | # File: IK_Solver_client.py 2 | # Auth: G. E. Deschaines 3 | # Date: 27 Apr 2015 4 | # Prog: Inverse Kinematics (IK) Solver client 5 | # Desc: Applies user selected IK solving technique to determine and 6 | # plot the motion of a jointed n-link chain in 2D or 3D space. 7 | # 8 | # This Python program presents a 3D implementation of inverse kinematic 9 | # solutions to end effector movement for a n-link, revolute-joint chain 10 | # towards a target using cyclic coordinate descent (CCD) [1], Jacobian 11 | # transpose method (JTM) [3], pseudo-inverse method (PIM) [1,2,3] or 12 | # damped least squares (DLS) [3] as detailed in the listed references. 13 | # 14 | # The program source is comprised of the following Python files. 15 | # 16 | # IK_Solver_client.py - main routine and animation plot functions 17 | # IK_Solver_nlink.py - IK solver n-link chain definition function 18 | # IK_Solver_class.py - IK solver iteration and state data accessors 19 | # IK_Solver_funcs.py - IK solver math methods and support functions 20 | # 21 | # The IK solution technique is user selectable, and the program can be 22 | # configured to record the plotted link chain motion to a video file 23 | # for animation playback (requires ffmpeg or avconv). 24 | # 25 | # References: 26 | # 27 | # [1] Benjamin Kenwright, "Practical 01: Inverse Kinematics", September 28 | # 2014, School of Computer Science, Edinburgh Napier University, 29 | # United Kingdom, Physics-Based Animation practical web available at 30 | # http://games.soc.napier.ac.uk/study/pba_practicals/Practical%2001%20-%20Inverse%20Kinematics.pdf 31 | # 32 | # [2] Ben Kenwright, "Real-Time Character Inverse Kinematics using 33 | # the Gauss-Seidel Iterative Approximation Method", July 2012, 34 | # CONTENT 2012, The Fourth International Conference on Creative 35 | # Content Technologies, web available at 36 | # http://www.thinkmind.org/index.php?view=article&articleid=content_2012_4_10_60013 37 | # 38 | # [3] Samuel R. Buss, "Introduction to Inverse Kinematics with Jacobian 39 | # Transpose, Pseudoinverse and Damped Least Squares methods", October 40 | # 2009, Department of Mathematics, University of California, San 41 | # Diego, unpublished article web available at 42 | # http://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/iksurvey.pdf 43 | # 44 | # Disclaimer: 45 | # 46 | # See DISCLAIMER 47 | 48 | import sys 49 | import os 50 | import time 51 | 52 | from math import ceil 53 | from socket import * 54 | from array import * 55 | from struct import * 56 | 57 | import warnings 58 | warnings.simplefilter(action='ignore', category=FutureWarning) 59 | 60 | try: 61 | import numpy as np 62 | import scipy.linalg as la 63 | except ImportError: 64 | print("* Error: NumPy and SciPy packages required.") 65 | print(" Suggest installing the SciPy stack.") 66 | sys.exit() 67 | 68 | try: 69 | import matplotlib.pyplot as plt 70 | import matplotlib.animation as animation 71 | from matplotlib.lines import Line2D 72 | from mpl_toolkits.mplot3d.art3d import Line3D 73 | except ImportError: 74 | print("* Error: matplotlib package required.") 75 | print(" Suggest installing the SciPy stack.") 76 | sys.exit() 77 | 78 | try: 79 | from IK_Solver_nlink import * 80 | from IK_Solver_class import * 81 | except ImportError: 82 | print("* Error: IK_Solver_class.py required.") 83 | sys.exit() 84 | 85 | # Handle Python 2.x and 3.x raw_input. 86 | 87 | try: 88 | type(raw_input) 89 | except NameError: 90 | def raw_input(prompt): 91 | return input(prompt) 92 | 93 | # Sockets data packing/unpacking functions 94 | 95 | def pack_mouse_bxy(buffer,b,x,y): 96 | ioff = 0 97 | pack_into('c',buffer,ioff,'M'.encode()) 98 | ioff = ioff + calcsize('c') 99 | pack_into('I',buffer,ioff,b) 100 | ioff = ioff + calcsize('I') 101 | pack_into('ff',buffer,ioff,x,y) 102 | ioff = ioff + calcsize('ff') 103 | return ioff 104 | 105 | def pack_target_pos(buffer,x,y,z): 106 | ioff = 0 107 | pack_into('c',buffer,ioff,'P'.encode()) 108 | ioff = ioff + calcsize('c') 109 | pack_into('fff',buffer,ioff,x,y,z) 110 | ioff = ioff + calcsize('fff') 111 | return ioff 112 | 113 | def pack_target_vel(buffer,vx,vy,vz): 114 | ioff = 0 115 | pack_into('c',buffer,ioff,'V'.encode()) 116 | ioff = ioff + calcsize('c') 117 | pack_into('fff',buffer,ioff,vx,vy,vz) 118 | ioff = ioff + calcsize('fff') 119 | return ioff 120 | 121 | def unpack_points_xy(buffer,ioff): 122 | (n,) = unpack_from('I',buffer,ioff) 123 | ioff = ioff + calcsize('I') 124 | X = np.zeros(n) 125 | Y = np.zeros(n) 126 | for i in range(0,n) : 127 | (X[i],Y[i])= unpack_from('ff',buffer,ioff) 128 | ioff = ioff + calcsize('ff') 129 | return (ioff, X, Y) 130 | 131 | def unpack_points_xyz(buffer,ioff): 132 | (n,) = unpack_from('I',buffer,ioff) 133 | ioff = ioff + calcsize('I') 134 | X = np.zeros(n) 135 | Y = np.zeros(n) 136 | Z = np.zeros(n) 137 | for i in range(0,n) : 138 | (X[i],Y[i],Z[i])= unpack_from('fff',buffer,ioff) 139 | ioff = ioff + calcsize('fff') 140 | return (ioff, X, Y, Z) 141 | 142 | def sendTgtVel(n): 143 | global Plot3D, tcpCliSock, BUFSIZ 144 | # Set target's velocit based on frame number. 145 | if Plot3D == 0 : 146 | tVel = np.array([-0.1, 0.0, 0.0]) 147 | else : 148 | tVel = np.array([ 0.0,-0.2, 0.0]) 149 | # Flush buffer 150 | buffer = array('B',(0 for i in range(0,16))) 151 | # Pack target position and send 152 | ioff = pack_target_vel(buffer,tVel[0],tVel[1],tVel[2]) 153 | tcpCliSock.send(buffer) 154 | # Wait for acknowledge 155 | data = tcpCliSock.recv(16) 156 | 157 | def sendTgtPos(n): 158 | global Plot3D, tcpCliSock, BUFSIZ 159 | # Set target's initial position 160 | if Plot3D == 0 : 161 | tPos = np.array([3.0,4.0,0.0]) 162 | else : 163 | tPos = np.array([3.0,2.5,2.0]) 164 | # Flush buffer 165 | buffer = array('B',(0 for i in range(0,16))) 166 | # Pack target position and send 167 | ioff = pack_target_pos(buffer,tPos[0],tPos[1],tPos[2]) 168 | tcpCliSock.send(buffer) 169 | # Wait for acknowledge 170 | data = tcpCliSock.recv(16) 171 | 172 | # Animation plotting functions. 173 | 174 | def onclick(e): 175 | """ 176 | button press event handler 177 | """ 178 | tcpCliSock, BUFSIZ 179 | 180 | # Flush buffer 181 | buffer = array('B',(0 for i in range(0,16))) 182 | 183 | # Pack mouse event and send 184 | ioff = pack_mouse_bxy(buffer,e.button,e.xdata,e.ydata) 185 | tcpCliSock.send(buffer) 186 | 187 | # Wait for acknowledge 188 | data = tcpCliSock.recv(16) 189 | 190 | def init2D(): 191 | """ 192 | init_func for 2D plotting 193 | """ 194 | global line1, line2, line3, line4, line5, time_text 195 | 196 | line1.set_data([], []) 197 | line2.set_data([], []) 198 | line3.set_data([], []) 199 | line4.set_data([], []) 200 | line5.set_data([], []) 201 | time_text.set_text('') 202 | 203 | # return artists in drawing order 204 | return line5, line1, line3, line4, line2, time_text 205 | 206 | def animate2D(n): 207 | """ 208 | animation function for 2D plotting 209 | """ 210 | global Server, tcpCliSock, BUFSIZ 211 | global ik_solver, tdel 212 | global line1, line2, line3, line4, line5, time_text 213 | 214 | # IK Solver update 215 | 216 | if Server == 0 : 217 | if n == 0 : 218 | ik_solver.set_target_pos( 3.0, 4.0, 0.0) 219 | ik_solver.set_target_vel(-0.1, 0.0, 0.0) 220 | ik_solver.step(tdel) 221 | if n == 0 : 222 | ik_solver.zero() 223 | time = ik_solver.tsolve 224 | (X0,Y0) = ik_solver.get_points_x0y0() 225 | (X,Y) = ik_solver.get_points_xy() 226 | (XT,YT) = ik_solver.get_target_xy() 227 | (XPT,YPT) = ik_solver.get_predtgt_xy() 228 | else : 229 | if n == 0 : 230 | sendTgtPos(n) 231 | sendTgtVel(n) 232 | data = 'update' + ' ' + str(n) 233 | tcpCliSock.send(data.encode()) 234 | buffer = tcpCliSock.recv(BUFSIZ) 235 | ioff = 0 236 | (time,)= unpack_from('f',buffer,ioff) 237 | ioff = ioff + calcsize('f') 238 | (ioff,X0,Y0) = unpack_points_xy(buffer,ioff) 239 | (ioff,X,Y) = unpack_points_xy(buffer,ioff) 240 | (ioff,XT,YT) = unpack_points_xy(buffer,ioff) 241 | (ioff,XPT,YPT) = unpack_points_xy(buffer,ioff) 242 | 243 | # Set line data and time text for plotting 244 | 245 | i = np.size(X0) - 1 246 | X0PT = np.array([X0[i],XPT[0]]) 247 | Y0PT = np.array([Y0[i],YPT[0]]) 248 | 249 | line1.set_data(X0,Y0) 250 | line2.set_data(X,Y) 251 | line3.set_data(XT,YT) 252 | line4.set_data(XPT,YPT) 253 | line5.set_data(X0PT,Y0PT) 254 | time_text.set_text('time = %.3f' % time) 255 | 256 | # return artists in drawing order 257 | return line5, line1, line3, line4, line2, time_text 258 | 259 | def init3D(): 260 | """ 261 | init_func for 3D plotting 262 | """ 263 | global line1, line2, line3, line4, line5,\ 264 | line6, line7, line8, line9, time_text 265 | 266 | line1.set_data(np.array([]), np.array([])) 267 | line1.set_3d_properties(np.array([])) 268 | line2.set_data(np.array([]), np.array([])) 269 | line2.set_3d_properties(np.array([])) 270 | line3.set_data(np.array([]), np.array([])) 271 | line3.set_3d_properties(np.array([])) 272 | line4.set_data(np.array([]), np.array([])) 273 | line4.set_3d_properties(np.array([])) 274 | line5.set_data(np.array([]), np.array([])) 275 | line5.set_3d_properties(np.array([])) 276 | line6.set_data(np.array([]), np.array([])) 277 | line6.set_3d_properties(np.array([])) 278 | line7.set_data(np.array([]), np.array([])) 279 | line7.set_3d_properties(np.array([])) 280 | line8.set_data(np.array([]), np.array([])) 281 | line8.set_3d_properties(np.array([])) 282 | line9.set_data(np.array([]), np.array([])) 283 | line9.set_3d_properties(np.array([])) 284 | time_text.set_text('') 285 | 286 | # return artists in drawing order 287 | return line7, line8, line9, line4, line5,\ 288 | line6, line1, line3, line2, time_text 289 | 290 | def animate3D(n): 291 | """ 292 | animation function for 3D plotting 293 | """ 294 | global Server, tcpClisock, BUFSIZ 295 | global ik_solver, tdel 296 | global line1, line2, line3, line4, line5,\ 297 | line6, line7, line8, line9, time_text 298 | 299 | # IK Solver update 300 | 301 | if Server == 0 : 302 | if n == 0 : 303 | ik_solver.set_target_pos( 3.0, 2.5, 2.0) 304 | ik_solver.set_target_vel( 0.0,-0.2, 0.0) 305 | ik_solver.step(tdel) 306 | if n == 0 : 307 | ik_solver.zero() 308 | time = ik_solver.tsolve 309 | (X0,Y0,Z0) = ik_solver.get_points_x0y0z0() 310 | (X,Y,Z) = ik_solver.get_points_xyz() 311 | (XT,YT,ZT) = ik_solver.get_target_xyz() 312 | (XE,YE,ZE) = ik_solver.get_endeff_xyz() 313 | (XPT,YPT,ZPT) = ik_solver.get_predtgt_xyz() 314 | else : 315 | if n == 0 : 316 | sendTgtPos(n) 317 | sendTgtVel(n) 318 | data = 'update' + ' ' + str(n) 319 | tcpCliSock.send(data.encode()) 320 | buffer = tcpCliSock.recv(BUFSIZ) 321 | ioff = 0 322 | (time,)= unpack_from('f',buffer,ioff) 323 | ioff = ioff + calcsize('f') 324 | (ioff,X0,Y0,Z0) = unpack_points_xyz(buffer,ioff) 325 | (ioff,X,Y,Z) = unpack_points_xyz(buffer,ioff) 326 | (ioff,XT,YT,ZT) = unpack_points_xyz(buffer,ioff) 327 | (ioff,XE,YE,ZE) = unpack_points_xyz(buffer,ioff) 328 | (ioff,XPT,YPT,ZPT) = unpack_points_xyz(buffer,ioff) 329 | 330 | # Set line data and time text for plotting 331 | 332 | line1.set_data(X0,Y0) 333 | line1.set_3d_properties(Z0) 334 | line2.set_data(X,Y) 335 | line2.set_3d_properties(Z) 336 | line3.set_data(XT,YT) 337 | line3.set_3d_properties(ZT) 338 | 339 | i = np.size(X0) - 1 340 | X0P = np.array([X0[i],X0[i],ax.get_xlim()[0]],dtype=object) 341 | Y0P = np.array([Y0[i],ax.get_ylim()[1],Y0[i]],dtype=object) 342 | Z0P = np.array([ax.get_zlim()[0],Z0[i],Z0[i]],dtype=object) 343 | line4.set_data(X0P,Y0P) 344 | line4.set_3d_properties(Z0P) 345 | XEP = np.array([XE,XE,ax.get_xlim()[0]],dtype=object) 346 | YEP = np.array([YE,ax.get_ylim()[1],YE],dtype=object) 347 | ZEP = np.array([ax.get_zlim()[0],ZE,ZE],dtype=object) 348 | line5.set_data(XEP,YEP) 349 | line5.set_3d_properties(ZEP) 350 | XTP = np.array([XPT,XPT,ax.get_xlim()[0]],dtype=object) 351 | YTP = np.array([YPT,ax.get_ylim()[1],YPT],dtype=object) 352 | ZTP = np.array([ax.get_zlim()[0],ZPT,ZPT],dtype=object) 353 | line6.set_data(XTP,YTP) 354 | line6.set_3d_properties(ZTP) 355 | X0TP0 = np.array([X0P[0],XTP[0]],dtype=object) 356 | Y0TP0 = np.array([Y0P[0],YTP[0]],dtype=object) 357 | Z0TP0 = np.array([Z0P[0],ZTP[0]],dtype=object) 358 | line7.set_data(X0TP0,Y0TP0) 359 | line7.set_3d_properties(Z0TP0) 360 | X0TP1 = np.array([X0P[1],XTP[1]],dtype=object) 361 | Y0TP1 = np.array([Y0P[1],YTP[1]],dtype=object) 362 | Z0TP1 = np.array([Z0P[1],ZTP[1]],dtype=object) 363 | line8.set_data(X0TP1,Y0TP1) 364 | line8.set_3d_properties(Z0TP1) 365 | X0TP2 = np.array([X0P[2],XTP[2]],dtype=object) 366 | Y0TP2 = np.array([Y0P[2],YTP[2]],dtype=object) 367 | Z0TP2 = np.array([Z0P[2],ZTP[2]],dtype=object) 368 | line9.set_data(X0TP2,Y0TP2) 369 | line9.set_3d_properties(Z0TP2) 370 | time_text.set_text('time = %.3f' % time) 371 | 372 | # return artists in drawing order 373 | return line7, line8, line9, line4, line5,\ 374 | line6, line1, line3, line2, time_text 375 | 376 | 377 | # IK_Solver main. 378 | 379 | if __name__ == '__main__': 380 | 381 | # Processing selectors 382 | 383 | Server = 1 # use IK Solver server 384 | IKmethod = 0 # selected IK solver method 385 | Plot3D = 0 # plot in 3D flag 386 | Record = 0 # record movie flag 387 | 388 | # IK Server tcp connection parameters 389 | 390 | HOST = '127.0.0.1' 391 | PORT = 21000 392 | ADDR = (HOST, PORT) 393 | BUFSIZ = 512 394 | 395 | # Check program arguments for IK method, Plot3D and Record values. 396 | 397 | sval = '0' 398 | if ( len(sys.argv) > 1 ) : 399 | sval = sys.argv[1] 400 | if ( len(sys.argv) > 2 ) : 401 | Plot3D = eval(sys.argv[2]) 402 | if ( len(sys.argv) > 3 ) : 403 | Record = eval(sys.argv[3]) 404 | 405 | # Request IK method selection (if not already given). 406 | 407 | prompt = 'Select IK Solver (1=CCD,2=JTM,3=PIM2,4=PIM3,5=DLS)? ' 408 | while IKmethod == 0 : 409 | if sval == '0' : 410 | sval = raw_input(prompt) 411 | if sval == '1' : 412 | IKmethod = UseCCD 413 | title = 'IK_Solver - Cyclic Coordinate Descent' 414 | elif sval == '2' : 415 | IKmethod = UseJTM 416 | title = 'IK_Solver - Using Jacobian Transpose Method' 417 | elif sval == '3' : 418 | IKmethod = UsePIM2 419 | title = 'IK_Solver - Using Pseudo-Inverse Method [ref 2]' 420 | elif sval == '4' : 421 | IKmethod = UsePIM3 422 | title = 'IK_Solver - Using Pseudo-Inverse Method [ref 3]' 423 | elif sval == '5' : 424 | IKmethod = UseDLS 425 | title = 'IK_Solver - Using Damped Least Squares [ref 3]' 426 | elif sval == '6' : 427 | IKmethod = UsePIM3dH 428 | title = 'IK_Solver - Using Pseudo-Inverse Method with null space control [ref 3]' 429 | elif sval == '7' : 430 | IKmethod = UseDLSdH 431 | title = 'IK_Solver - Using Damped Least Squares with null space control [ref 3]' 432 | else : 433 | sval = '0' 434 | IKmethod = 0 435 | 436 | # Define link chain structure and parameters. 437 | 438 | (a, u, q0, qmin, qmax, dqlim) = get_nlink_chain(Plot3D) 439 | 440 | # Instantiate an IK_Solver or connect to an IK Solver server. 441 | 442 | ik_server = 0 443 | if Server == 0 : 444 | tcpCliSock = 0 445 | ik_solver = IK_Solver(Plot3D,IKmethod,a,u,q0,qmin,qmax,dqlim) 446 | else : 447 | tcpCliSock = socket(family=AF_INET, type=SOCK_STREAM) 448 | try : 449 | tcpCliSock.connect(ADDR) 450 | data = 'start' + ' ' + str(Plot3D) + ' ' + str(IKmethod) 451 | tcpCliSock.send(data.encode()) 452 | raw_data = tcpCliSock.recv(BUFSIZ) 453 | data = raw_data.decode() 454 | print(data) 455 | except : 456 | print('Could not connect to an IK Solver server.') 457 | print('Will use a locally instantiated IK Solver.') 458 | tcpCliSock = 0 459 | Server = 0 460 | ik_solver = IK_Solver(Plot3D,IKmethod,a,u,q0,qmin,qmax,dqlim) 461 | 462 | # If recording animation, assign an animation writer. 463 | 464 | Writer = None 465 | if Record == 1 : 466 | try: 467 | Writer = animation.writers['ffmpeg'] 468 | except KeyError: 469 | try: 470 | Writer = animation.writers['avconv'] 471 | except KeyError: 472 | print("Cannot record animation, no animation writers available!") 473 | print("Try installing ffmpeg or avconv.") 474 | Record = 0 475 | 476 | # Instantiate a matplotlib pyplot figure. 477 | 478 | if Record == 0 : 479 | fig = plt.figure(figsize=(8,6), dpi=80) 480 | else : 481 | fig = plt.figure(figsize=(6,4), dpi=80) 482 | 483 | # Specify plotting objects and parameters. 484 | 485 | if Plot3D == 0 : 486 | ax = fig.add_subplot(111, autoscale_on=False) 487 | line1 = Line2D([],[],color='g',ls='-',lw=1.0, # link chain at start 488 | marker='o',mew=1.0,mec='k',mfc='g') 489 | line2 = Line2D([],[],color='b',ls='-',lw=2.0, # link chain in motion 490 | marker='o',mew=1.0,mec='k',mfc='b') 491 | line3 = Line2D([],[],color='r',ls=' ',lw=2.0, # current target location 492 | marker='x',mew=2.0,mec='r',mfc='r') 493 | line4 = Line2D([],[],color='m',ls=' ',lw=2.0, # predicted target location 494 | marker='x',mew=2.0,mec='m',mfc='m') 495 | line5 = Line2D([],[],color='k',ls=':',lw=1.0, # eff to predicted tgt line 496 | marker=' ') 497 | time_text = ax.text(0.02, 0.95, '', transform=ax.transAxes) 498 | ax.set_title(title) 499 | ax.set_xlabel('X') 500 | ax.set_ylabel('Y') 501 | ax.set_xlim(-6,6) 502 | ax.set_ylim(-6,6) 503 | ax.set_aspect('equal') 504 | ax.grid() 505 | ax.add_line(line1) 506 | ax.add_line(line2) 507 | ax.add_line(line3) 508 | ax.add_line(line4) 509 | ax.add_line(line5) 510 | fig.canvas.draw() 511 | else : 512 | ax = fig.add_subplot(111, projection='3d') 513 | line1 = Line3D([],[],[],color='g',ls='-',lw=1.0, # link chain start in 3D 514 | marker='o',mew=1.0,mec='k',mfc='g') 515 | line2 = Line3D([],[],[],color='b',ls='-',lw=2.0, # link chain motion in 3D 516 | marker='o',mew=1.0,mec='k',mfc='b') 517 | line3 = Line3D([],[],[],color='r',ls=' ',lw=2.0, # target in 3D space 518 | marker='x',mew=2.0,mec='r',mfc='r') 519 | line4 = Line3D([],[],[],color='g',ls=' ',lw=1.0, # eff start in XYZ planes 520 | marker='o',mew=1.0,mec='g',mfc='w') 521 | line5 = Line3D([],[],[],color='b',ls=' ',lw=1.0, # eff motion in XYZ planes 522 | marker='o',mew=1.0,mec='b',mfc='w') 523 | line6 = Line3D([],[],[],color='m',ls=' ',lw=1.0, # predicted target in XYZ planes 524 | marker='x',mew=1.0,mec='m',mfc='m') 525 | line7 = Line3D([],[],[],color='k',ls=':',lw=1.0, # eff to pred. tgt line in XY plane 526 | marker=' ') 527 | line8 = Line3D([],[],[],color='k',ls=':',lw=1.0, # eff to pred. tgt line in XZ plane 528 | marker=' ') 529 | line9 = Line3D([],[],[],color='k',ls=':',lw=1.0, # eff to pred. tgt line in YZ plane 530 | marker=' ') 531 | time_text = ax.text(-6.0, -7.0, 6.5, '',) 532 | ax.set_title(title) 533 | ax.set_xlabel('X') 534 | ax.set_ylabel('Y') 535 | ax.set_zlabel('Z') 536 | ax.set_xlim3d([-5,5]) 537 | ax.set_ylim3d([-5,5]) 538 | ax.set_zlim3d([ 0,5]) 539 | #ax.set_aspect('equal') 540 | ax.grid() 541 | ax.add_line(line1) 542 | ax.add_line(line2) 543 | ax.add_line(line3) 544 | ax.add_line(line4) 545 | ax.add_line(line5) 546 | ax.add_line(line6) 547 | ax.add_line(line7) 548 | ax.add_line(line8) 549 | ax.add_line(line9) 550 | fig.canvas.draw() 551 | 552 | # Determine animation interval time. 553 | 554 | FPS = 10 555 | tdel = 1.0/FPS 556 | tdel_msec = 1000.0*tdel 557 | tmsec0 = 1000.0*time.clock() 558 | if Plot3D == 0 : 559 | animate2D(0) 560 | else : 561 | animate3D(0) 562 | tmsec1 = 1000.0*time.clock() 563 | tstep = tmsec1 - tmsec0 564 | interval = ceil(tmsec1-tmsec0) # allows faster than real-time 565 | #interval = tdel_msec - tstep # approximates real-time 566 | print("tdel_msec = %8.3f" % tdel_msec) 567 | print("t1 - t0 = %8.3f" % tstep) 568 | print("interval = %8.3f" % interval) 569 | 570 | # Assign mouse button press handler. 571 | 572 | if Server == 0 : 573 | cid = fig.canvas.mpl_connect('button_press_event', ik_solver.onclick) 574 | else : 575 | cid = fig.canvas.mpl_connect('button_press_event', onclick) 576 | 577 | # Specify animation parameters and assign functions. 578 | 579 | nframes = None 580 | blit = False # blitting not working well with Matplotlib v1.5.1+ 581 | if Record == 1 : 582 | nframes = 14*FPS 583 | blit = False 584 | 585 | if Plot3D == 0 : 586 | anim = animation.FuncAnimation(fig, animate2D, init_func=init2D, 587 | frames=nframes, blit=blit, 588 | interval=interval, repeat=False) 589 | else : 590 | anim = animation.FuncAnimation(fig, animate3D, init_func=init3D, 591 | frames=nframes, blit=blit, 592 | interval=interval, repeat=False) 593 | 594 | # Begin. 595 | 596 | if Record == 0 or Writer is None: 597 | if Plot3D == 0 : 598 | print("Move cursor into plot region and press middle mouse button") 599 | print("to initiate IK solver for selected target location.") 600 | else : 601 | print("With cursor in the plot region, press middle mouse button") 602 | print("to initiate IK solver for randomly positioned target.") 603 | plt.show() 604 | else : 605 | plt.ioff() 606 | print("Creating animation video; presentation will begin shortly ...") 607 | writer = Writer(fps=FPS, codec='libx264', 608 | metadata=dict(artist='IK_Solver'), 609 | bitrate=-1) 610 | print("Performing IK solver iteration...") 611 | filename = 'IK_Solver_' + str(IKmethod) + '.mp4' 612 | anim.save(filename, writer=writer) 613 | print("Animation saved in file %s" % filename) 614 | plt.show() 615 | 616 | # Terminate and exit. 617 | 618 | if ik_server : 619 | del ik_server 620 | if tcpCliSock : 621 | prompt = 'Do you want to stop the IK_Solver server [Y|y]? ' 622 | sval = raw_input(prompt) 623 | if sval == 'Y' or sval == 'y' : 624 | data = 'quit' 625 | else : 626 | data = 'stop' 627 | tcpCliSock.send(data.encode()) 628 | tcpCliSock.close() 629 | -------------------------------------------------------------------------------- /MATLAB/IK_Solver.m: -------------------------------------------------------------------------------- 1 | % File: IK_Solver.m 2 | % Auth: G. E. Deschaines 3 | % Date: 27 Feb 2015 4 | % Prog: Inverse Kinematics (IK) Solver 5 | % Desc: Applies user selected IK solving technique to determine and 6 | % plot the motion of a jointed n-link chain in 2D or 3D space. 7 | % 8 | % This MATLAB program presents a 3D implementation of inverse kinematic 9 | % solutions to effector movement for a n-link, revolute-joint chain 10 | % towards a target using cyclic coordinate descent (CCD) [1], Jacobian 11 | % transpose method (JTM) [3], pseudo-inverse method (PIM) [1,2,3] or 12 | % damped least squares (DLS) [3] as detailed in the listed references. 13 | % The program is comprised of the following script files. 14 | % 15 | % IK_Solver.m - main program utilizing the following functions: 16 | % ik_jtm.m - apply IK Jacobian transpose method 17 | % ik_pim2.m - apply IK pseudo-inverse method from ref [2] 18 | % ik_pim3.m - apply IK pseudo-inverse method from ref [3] 19 | % ik_dls - apply IK damped least squares technique 20 | % solve_chk.m - IK solver solution check 21 | % time_to_goal.m - time to reach target 22 | % closing_gain.m - compute closing gain 23 | % get_endeff_rot.m - get end effector rotation in 2D space 24 | % get_endeff_ypr.m - get end effector yaw, pitch, roll in 3D space 25 | % avelT_wrt_jointm.m - ang. vel. of target wrt to joint m 26 | % avelE_wrt_jointm.m - ang. vel. of end effector wrt to joint m 27 | % null_space_control.m - compute null space control vector 28 | % jacobian.m - compute Jacobian matrix 29 | % rotation.m - compute coordinate rotation matrix 30 | % transform.m - transform coordinates from local to world space 31 | % clamp_rot.m - clamp joint rotations to specified limits 32 | % plot_xy.m - extract link plot x,y coordinates for 2D plotting 33 | % plot_xyz.m - extract link plot x,y,z coordinates for 3D plotting 34 | % angle_chk.m - debug display of joint rotation angle checks 35 | % isOctave.m - returns true if script is being executed by Octave 36 | % 37 | % The following functions are not required by MATLAB, but are used 38 | % by Octave to emulate MATLAB getframe and movie2avi functions. These 39 | % functions do not allow smooth animation as in MATLAB. There will 40 | % be disruptive graphic flickering and figure jumping as putimage 41 | % saves an image of the figure to a file. 42 | % 43 | % putimage.m - writes animation frame image to file 44 | % getimage.m - reads animation frame image from file 45 | % image2avi.m - converts sequence of frame image files to avi video 46 | % 47 | % The IK solution technique is user selectable, and the program can be 48 | % configured to record the plotted link chain motion for animation 49 | % playback and saving as a video file. 50 | % 51 | % References: 52 | % 53 | % [1] Benjamin Kenwright, "Practical 01: Inverse Kinematics", September 54 | % 2014, School of Computer Science, Edinburgh Napier University, 55 | % United Kingdom, Physics-Based Animation practical web available at 56 | % http://games.soc.napier.ac.uk/study/pba_practicals/Practical%2001%20-%20Inverse%20Kinematics.pdf 57 | % 58 | % [2] Ben Kenwright, "Real-Time Character Inverse Kinematics using 59 | % the Gauss-Seidel Iterative Approximation Method", July 2012, 60 | % CONTENT 2012, The Fourth International Conference on Creative 61 | % Content Technologies, web available at 62 | % http://www.thinkmind.org/index.php?view=article&articleid=content_2012_4_10_60013 63 | % 64 | % [3] Samuel R. Buss, "Introduction to Inverse Kinematics with Jacobian 65 | % Transpose, Pseudoinverse and Damped Least Squares methods", October 66 | % 2009, Department of Mathematics, University of California, San 67 | % Diego, unpublished article web available at 68 | % http://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/iksurvey.pdf 69 | % 70 | % Disclaimer: 71 | % 72 | % See IK_Solver/DISCLAIMER 73 | 74 | clear variables 75 | close all 76 | 77 | more off 78 | 79 | %#ok<*NOPTS> 80 | 81 | global rpd; 82 | global dpr; 83 | 84 | rpd = pi/180; % radians per degree conversion constant 85 | dpr = 180/pi; % degrees per radian conversion constant 86 | 87 | global p360rad; % globals used in clamp_rot function 88 | global n360rad; % 89 | global p180rad; % 90 | global n180rad; % 91 | 92 | p360rad = 360*rpd; 93 | n360rad = -360*rpd; 94 | p180rad = 180*rpd; 95 | n180rad = -180*rpd; 96 | 97 | Plot3D = 1; % plot in 3D flag 98 | Record = 0; % record movie flag 99 | 100 | UseCCD = 1; % use cyclic coordinate descent 101 | UseJTM = 2; % use jacobian transpose method 102 | UsePIM2 = 3; % use pseudo-inverse method from ref [2] 103 | UsePIM3 = 4; % use pseudo-inverse method from ref [3] 104 | UseDLS = 5; % use damped least squares from ref [3] 105 | UsePIM3dH = 6; % use pseudo-inverse method from ref [3] with null space control 106 | UseDLSdH = 7; % use damped least squares from ref [3] with null space control 107 | IKmethod = 0; % selected IK solver method 108 | 109 | prompt = 'Select IK Solver (1=CCD,2=JTM,3=PIM2,4=PIM3,5=DLS,6=PIM3dH,7=DLSdH)? '; 110 | while IKmethod == 0 111 | str = input(prompt,'s'); 112 | switch str 113 | case '1' 114 | title = 'IK_Solver - Cyclic Coordinate Descent'; 115 | IKmethod = UseCCD; 116 | case '2' 117 | title = 'IK_Solver - Using Jacobian Transpose Method'; 118 | IKmethod = UseJTM; 119 | case '3' 120 | title = 'IK_Solver - Using Pseudo-Inverse Method [ref 2]'; 121 | IKmethod = UsePIM2; 122 | case '4' 123 | title = 'IK_Solver - Using Pseudo-Inverse Method [ref 3]'; 124 | IKmethod = UsePIM3; 125 | case '5' 126 | title = 'IK_Solver - Using Damped Least Squares [ref 3]'; 127 | IKmethod = UseDLS; 128 | case '6' 129 | title = 'IK_Solver - Using Pseudo-Inverse Method [ref 3] with null space control'; 130 | IKmethod = UsePIM3dH; 131 | case '7' 132 | title = 'IK_Solver - Using Damped Least Squares [ref 3] with null space control'; 133 | IKmethod = UseDLSdH; 134 | otherwise 135 | fprintf('Invalid entry\n'); 136 | IKmethod = 0; 137 | end 138 | end 139 | fprintf('%s\n',title); 140 | fprintf('On the plot, use the mouse buttons:\n'); 141 | if Plot3D == 0 142 | fprintf(' + left to select target location\n'); 143 | else 144 | fprintf(' + left to randomly locate target\n'); 145 | end 146 | fprintf(' + right to stop the IK solver\n'); 147 | fprintf('Wait until program exits to close plot window.\n'); 148 | 149 | % Jointed N-Link Chain Pictogram 150 | % + 151 | % uz1 uz2 uz3 uzN . (a)N+1 152 | % | uy1 | uy2 | uy3 | uyN . ^end 153 | % | / | / | / | / . | effector 154 | % |/ |/ |/ |/ . | 155 | % base>+------>ux1 -+------>ux2 -+------>ux3 ... -+------>uxN | 156 | % (a)0 (a)1 (a)2 (a)N | 157 | % ^joint_1 ^joint_2 ^joint_3 ^joint_N | 158 | % | & link_1 | & link_2 | & link_3 | & link_N | 159 | % | | | | | 160 | % +----< preceding link body space [x,y,z] >---+ 161 | 162 | if Plot3D == 0 163 | a = {[0.0,0.0,0.0],... % <- joint 1 + set of link chain attachment 164 | [2.0,0.0,0.0],... % <- joint 2 | points in body coordinates, 165 | [2.0,0.0,0.0],... % <- joint 3 | except for the base given in 166 | [1.0,0.0,0.0],... % <- joint 4 | world space coordinates 167 | [0.0,0.0,0.0],... % <- joint 5 / 168 | [1.0,0.0,0.0]}; % <- end effector 169 | u = {[0.0,0.0,1.0],... % <- joint 1 + set of joint rotation axis unit 170 | [0.0,0.0,1.0],... % <- joint 2 | vectors in link body coordinates 171 | [0.0,0.0,1.0],... % <- joint 3 | 172 | [1.0,0.0,0.0],... % <- joint 4 | 173 | [0.0,0.0,1.0]}; % <- joint 5 / 174 | q0 = [ 25*rpd,... % <- joint 1 + initial joint rotations (radians) 175 | 15*rpd,... % <- joint 2 | 176 | 10*rpd,... % <- joint 3 | 177 | 0*rpd,... % <- joint 4 | 178 | 5*rpd]; % <- joint 5 / 179 | qmin = [-360*rpd,... % <- joint 1 + joint minimum rotations (radians) 180 | -135*rpd,... % <- joint 2 | 181 | -60*rpd,... % <- joint 3 | 182 | -360*rpd,... % <- joint 4 | 183 | -160*rpd]; % <- joint 5 / 184 | qmax = [ 360*rpd,... % <- joint 1 + joint maximum rotations (radians) 185 | 135*rpd,... % <- joint 2 | 186 | 60*rpd,... % <- joint 3 | 187 | 360*rpd,... % <- joint 4 | 188 | 160*rpd]; % <- joitn 5 / 189 | dqlim = [ 30*rpd,... % <- joint 1 + joint delta rotation limit (rad/sec) 190 | 30*rpd,... % <- joint 2 | 191 | 30*rpd,... % <- joint 3 | 192 | 30*rpd,... % <- joint 4 | 193 | 30*rpd]; % <- joitn 5 / 194 | else 195 | a = {[0.0,0.0,0.0],... % <- joint 1 + set of link chain attachment 196 | [0.0,0.0,2.0],... % <- joint 2 | points in body coordinates, 197 | [2.0,0.0,0.0],... % <- joint 3 | except for the base given in 198 | [2.0,0.0,0.0],... % <- joint 4 | world space coordinates 199 | [0.0,0.0,0.0],... % <- joint 5 / 200 | [0.5,0.0,0.0]}; % <- end effector 201 | u = {[0.0,0.0,1.0],... % <- joint 1 + set of joint rotation axis unit 202 | [0.0,1.0,0.0],... % <- joint 2 | vectors in link body coordinates 203 | [0.0,1.0,0.0],... % <- joint 3 | 204 | [0.0,1.0,0.0],... % <- joint 4 | 205 | [0.0,0.0,1.0]}; % <- joint 5 / 206 | q0 = [ 0*rpd,... % <- joint 1 + initial joint rotations (radians) 207 | -45*rpd,... % <- joint 2 | 208 | 45*rpd,... % <- joint 3 | 209 | 0*rpd,... % <- joint 4 | 210 | 45*rpd]; % <- joint 5 / 211 | qmin = [-360*rpd,... % <- joint 1 + joint minimum rotations (radians) 212 | -135*rpd,... % <- joint 2 | 213 | -135*rpd,... % <- joint 3 | 214 | -85*rpd,... % <- joint 4 | 215 | -120*rpd]; % <- joint 5 / 216 | qmax = [ 360*rpd,... % <- joint 1 + joint maximum rotations (radians) 217 | 135*rpd,... % <- joint 2 | 218 | 135*rpd,... % <- joint 3 | 219 | 85*rpd,... % <- joint 4 | 220 | 120*rpd]; % <- joitn 5 / 221 | dqlim = [ 30*rpd,... % <- joint 1 + joint delta rotation limit (rad/sec) 222 | 30*rpd,... % <- joint 2 | 223 | 30*rpd,... % <- joint 3 | 224 | 30*rpd,... % <- joint 4 | 225 | 30*rpd]; % <- joitn 5 / 226 | end 227 | na = length(a); % number of link body attachment points 228 | np = na; % number of link attachment world positions 229 | nq = length(q0); % number of link chain joints 230 | s = zeros(1,nq); % create array of link sizes 231 | for i = 1 : na-1 232 | s(i) = norm(a{i+1}); 233 | end 234 | efd = s(end); % size of end effector link 235 | 236 | % generalizations used in distance checks of iteration solve_chk function; 237 | % assumes joint rotation limits do not prevent a full linear extension. 238 | if Plot3D == 0 239 | dxy = sum(s(1:end)); % length of fully extended chain in xy plane 240 | dz = 0.0; % length of fully extended chain in z direction 241 | else 242 | % following assumes first link strictly in the z direction 243 | dxy = sum(s(2:end)); % length of fully extended chain in xy plane 244 | dz = sum(s(1:end)); % length of fully extended chain in z direction 245 | end 246 | 247 | q = q0; % vector of current joint rotations 248 | dq = zeros(1,nq); % joint rotation rate vector 249 | [p,w] = transform(na,q,u,a); % joint positions/rotations in world space 250 | ec = p{na}; % end effector current position vector 251 | 252 | % specify end effector target position and velocity 253 | if Plot3D == 0 254 | et = [ 3.0, 4.0, 0.0]; % end effector target position vector 255 | vt = [-0.1, 0.0, 0.0]; % target velocity vector 256 | else 257 | et = [ 3.0, 2.5, 2.0]; % end effector target position vector 258 | vt = [ 0.0,-0.2, 0.0]; % target velocity vector 259 | end 260 | 261 | pt = et; % predicted target intercept position vector 262 | h = 0.04; % IK iteration step size 263 | FPS = 10; % IK animation frame rate 264 | kfps = round((1/FPS)/h); % IK iteration count per frame for FPS rate 265 | ilim = round(30/h); % IK iteration limit for 30 seconds 266 | ni = 0; % iteration counter 267 | tsim = 0.0; % simulation time (sec) 268 | tgo = h; % time to goal (sec) 269 | sdel = 0.001; % PIM singularity damping constant from ref [2] 270 | sfac = 0.01; % PIM singularity threshold factor from ref [3] 271 | slam = 1.1; % DLS singularity damping factor from ref [3] 272 | dH = zeros(1,nq); % null space control vector 273 | derr = 0.04*efd; % allowable effector to target distance error 274 | perr = atan(derr/efd); % allowable effector to target pointing error 275 | 276 | text_tsim = @(t){ sprintf('time = %.3f', t) }; 277 | 278 | fig1 = figure('Name', title, 'NumberTitle', 'Off', ... 279 | 'position', [120 100 580 480]); 280 | lw = 'linewidth'; % abbreviation convenience 281 | if Plot3D == 0 282 | [X0,Y0] = plot_xy(np,p); 283 | plot([X0(na),et(1)],[Y0(na),et(2)], ... 284 | X0,Y0,'-og', ... 285 | pt(1),pt(2),' *m', et(1),et(2),' *r'); 286 | xlabel('X');ylabel('Y'); 287 | axis([-6 6 -6 6]); 288 | axis square; 289 | text_x = -6.0; 290 | text_y = 6.5; 291 | else 292 | [X0,Y0,Z0] = plot_xyz(np,p); 293 | ViewAz = 45.0; 294 | ViewEl = 45.0; 295 | plot3(X0,Y0,Z0,'-og', ... 296 | pt(1),pt(2),pt(3),' *m', et(1),et(2),et(3),' *r'); 297 | view(ViewAz,ViewEl); 298 | [ViewAz,ViewEl] = view(); 299 | xlabel('X');ylabel('Y');zlabel('Z'); 300 | axis([-5 5 -5 5 0 5]); 301 | text_x = 8.0; 302 | text_y = -2.0; 303 | end 304 | grid on; 305 | hold on; 306 | set(gca,'NextPlot','ReplaceChildren'); 307 | text(text_x,text_y,text_tsim(tsim),'fontweight','bold'); 308 | drawnow; 309 | 310 | % specific sized plot figure for consistancy between MATLAB/Octave 311 | if isOctave 312 | fig1pos = get(fig1, 'position'); 313 | else 314 | fig1pos = get(fig1, 'innerposition'); 315 | end 316 | 317 | FMT = "jpg"; % Set to "png" or "jpg" 318 | if Record == 1 319 | if ~exist("./images", 'dir') 320 | mkdir("./images"); 321 | end 322 | if isOctave 323 | sdir = ["./images/", FMT, "s"]; 324 | if ~exist(sdir, 'dir') 325 | mkdir(sdir); 326 | end 327 | end 328 | k = 1; 329 | if isOctave 330 | F(k,:) = putimage(fig1, k, FMT, fig1pos(3), fig1pos(4)); 331 | else 332 | F(k) = getframe(fig1, [0 0 fig1pos(3), fig1pos(4)]); 333 | end 334 | end 335 | 336 | % Load random number sequence. (For run reproducibility) 337 | if Plot3D > 0 338 | nnum = 0; 339 | if exist("rnum.mat", 'file') 340 | load("rnum.mat", "rnum"); 341 | nnum = length(rnum); 342 | end 343 | if nnum == 0 344 | nnum = 100; 345 | if isOctave 346 | rand ("seed", 987654321); % Octave 347 | rnum = rand(nnum,1); 348 | else 349 | rng(987654321); % Matlab 350 | rnum = rand(nnum,1); 351 | end 352 | end 353 | inum = 0; 354 | end 355 | 356 | %% Main loop over IK solutions 357 | 358 | button = 1; 359 | while button == 1 360 | ik = 1; % iteration count till next plot update 361 | while solve_chk(ni,ilim,p,ec,et,dxy,dz,derr,perr) == 0 362 | % Calculate IK solution 363 | if IKmethod == UseCCD 364 | gain = closing_gain(IKmethod,ec,et,vt); 365 | ept = et + gain*tgo*vt; 366 | dq = zeros(1,nq); 367 | for i = nq:-1:1 368 | pt = ept - p{i}; 369 | npt = norm(pt); 370 | pc = ec - p{i}; 371 | npc = norm(pc); 372 | ut = cross(cross(w{i},pc/npc), cross(w{i},pt/npt)); 373 | if norm(ut) > 0 374 | ut = ut/norm(ut); 375 | dqmax = min([acos((pt*(pc/npc).')/norm(pt)),dqlim(i)]); 376 | dq(i) = dqmax*(ut*(w{i}).'); 377 | else 378 | dq(i) = 0.0; 379 | end 380 | end 381 | else 382 | gain = closing_gain(IKmethod,ec,et,vt); 383 | de = et + gain*tgo*vt - ec; 384 | % Jt = jacobian(nq,w,p,et); 385 | Jc = jacobian(nq,w,p,ec); 386 | J = Jc; 387 | switch IKmethod 388 | case UseJTM 389 | [dq] = ik_jtm(J,de,dqlim); 390 | case UsePIM2 391 | [dq] = ik_pim2(J,de,sdel,dqlim); 392 | case UsePIM3 393 | [dq] = ik_pim3(J,de,sfac,dqlim,dH); 394 | case UseDLS 395 | [dq] = ik_dls(J,de,slam,dqlim,dH); 396 | case UsePIM3dH 397 | [dH] = null_space_control(Plot3D,et,vt,w,p,dq,q,u); 398 | [dq] = ik_pim3(J,de,sfac,dqlim,dH); 399 | case UseDLSdH 400 | [dH] = null_space_control(Plot3D,et,vt,w,p,dq,q,u); 401 | [dq] = ik_dls(J,de,slam,dqlim,dH); 402 | end 403 | end 404 | % Update link chain angles and positions 405 | q = q + h*dq; 406 | [q] = clamp_rot(nq,q,qmin,qmax); 407 | [p,w] = transform(na,q,u,a); 408 | %%angle_chk(q,u,p,et); 409 | % Update end effector current and target positions 410 | ec = p{na}; 411 | et = et + h*vt; 412 | tgo = time_to_goal(et,vt,w,p,dq); 413 | gain = closing_gain(IKmethod,ec,et,vt); 414 | pt = et + gain*tgo*vt; 415 | % Increment iteration and simulation time counters 416 | ni = ni + 1; 417 | tsim = tsim + h; 418 | % Decrement iteration count till next plot update 419 | ik = ik - 1; 420 | if ik == 0 421 | if Plot3D == 0 422 | [X,Y] = plot_xy(np,p); 423 | if isOctave 424 | plot([X0(na),pt(1)],[Y0(na),pt(2)],' -k', ... 425 | X0,Y0,'-og', X,Y,'-ob',lw,2, ... 426 | et(1),et(2),' *r', pt(1),pt(2),' *m'); 427 | else 428 | plot([X0(na),pt(1)],[Y0(na),pt(2)],' -k', ... 429 | X0,Y0,'-og', X,Y,'-ob', ... 430 | et(1),et(2),' *r', pt(1),pt(2),' *m', lw,2); 431 | end 432 | else 433 | [X,Y,Z] = plot_xyz(np,p); 434 | [ViewAz,ViewEl] = view(); 435 | if isOctave 436 | plot3(X0,Y0,Z0,'-og', X,Y,Z,'-ob',lw,2, ... 437 | et(1),et(2),et(3),' *r', pt(1),pt(2),pt(3),' *m'); 438 | else 439 | plot3(X0,Y0,Z0,'-og', X,Y,Z,'-ob', ... 440 | et(1),et(2),et(3),' *r', pt(1),pt(2),pt(3),' *m', lw,2); 441 | end 442 | view(ViewAz,ViewEl); 443 | end 444 | text(text_x,text_y,text_tsim(tsim),'fontweight','bold'); 445 | drawnow; 446 | if Record == 1 447 | k = k + 1; 448 | if isOctave 449 | F(k,:) = putimage(fig1, k, FMT, fig1pos(3), fig1pos(4)); 450 | else 451 | F(k) = getframe(fig1, [0 0 fig1pos(3), fig1pos(4)]); 452 | end 453 | end 454 | ik = kfps; % reset iteration count till next plot update 455 | end 456 | end % end of IK iteration loop 457 | 458 | % Display solution time and plot 459 | error = norm(ec - et); 460 | fprintf('Iteration time (sec) = %8.3f\nEffector position error = %8.4f\n',... 461 | tsim, error); 462 | if Plot3D == 0 463 | theta = get_endeff_rot(nq,q,u); 464 | fprintf('Effector rotation angle = %8.3f\n', theta*dpr) 465 | else 466 | [psi,theta,phi] = get_endeff_ypr(nq,q,u); 467 | fprintf('Effector yaw,pitch,roll = %8.3f, %8.3f, %8.3f\n',... 468 | psi*dpr, theta*dpr, phi*dpr) 469 | end 470 | if Plot3D == 0 471 | [X,Y] = plot_xy(np,p); 472 | if isOctave 473 | plot([X0(na),et(1)],[Y0(na),et(2)],' -k', ... 474 | X0,Y0,'-og', X,Y,'-ob',lw,2, et(1),et(2),' *r'); 475 | else 476 | plot([X0(na),et(1)],[Y0(na),et(2)],' -k', ... 477 | X0,Y0,'-og', X,Y,'-ob', et(1),et(2),' *r', lw,2); 478 | end 479 | X0 = X; Y0 = Y; 480 | else 481 | [X,Y,Z] = plot_xyz(np,p); 482 | [ViewAz,ViewEl] = view(); 483 | if isOctave 484 | plot3(X0,Y0,Z0,'-og', X,Y,Z,'-ob',lw,2, et(1),et(2),et(3),' *r'); 485 | else 486 | plot3(X0,Y0,Z0,'-og', X,Y,Z,'-ob', et(1),et(2),et(3),' *r', lw,2); 487 | end 488 | view(ViewAz,ViewEl) 489 | X0 = X; Y0 = Y; Z0 = Z; 490 | end 491 | text(text_x,text_y,text_tsim(tsim),'fontweight','bold'); 492 | drawnow; 493 | if Record == 1 494 | k = k + 1; 495 | if isOctave 496 | F(k,:) = putimage(fig1, k, FMT, fig1pos(3), fig1pos(4)); 497 | else 498 | F(k) = getframe(fig1, [0 0 fig1pos(3), fig1pos(4)]); 499 | end 500 | end 501 | % Wait for button press 502 | [Xm,Ym,button] = ginput(1); 503 | if Plot3D == 0 504 | et(1) = min(max(Xm,-6),6); 505 | et(2) = min(max(Ym,-6),6); 506 | et(3) = 0; 507 | else 508 | inum = inum + 1; 509 | if inum > nnum 510 | inum = 1; 511 | end 512 | theta = 2*pi*rnum(inum); 513 | r = 1.5 + 2.5*rnum(inum); 514 | et(1) = r*cos(theta); 515 | et(2) = r*sin(theta); 516 | et(3) = 1 + 4*rnum(inum); 517 | end 518 | pt = et; 519 | % Reset iteration counter and simulation time 520 | ni = 0; 521 | tsim = 0.0; 522 | tgo = h; 523 | dq = zeros(1,nq); % joint rotation rate vector 524 | dH = zeros(1,nq); % null space control vector 525 | end % end of button press loop 526 | 527 | %% Animation playback 528 | 529 | if Record == 1 530 | fprintf('IK_Solver - Playback of recorded movie frames\n'); 531 | fprintf('On the figure, use the mouse buttons:\n'); 532 | fprintf(' + left to replay the movie\n'); 533 | fprintf(' + right to exit the program\n'); 534 | figM = figure('Name', 'IK_Solver - Recorded Frames', ... 535 | 'NumberTitle', 'Off', ... 536 | 'toolbar', 'none'); 537 | fig1opos = get(fig1, 'outerposition'); 538 | sfac = fig1opos(4)/fig1pos(4); 539 | if isOctave 540 | set(figM, 'position', [720 100 fig1opos(3)*sfac fig1opos(4)]) 541 | else 542 | set(figM, 'outerposition', [720 100 fig1opos(3)*sfac fig1opos(4)]); 543 | end 544 | grid off; 545 | button = 1; 546 | while button == 1 547 | if isOctave 548 | for k = 1:length(F) 549 | img = getimage(F(k,1:end)); 550 | image([0 fig1pos(3)], [0 fig1pos(4)], img, 'clipping', "on"); 551 | figMpos = get(figM, 'position'); 552 | set(figM, 'position', [figMpos(1) figMpos(2) fig1opos(3)*sfac fig1opos(4)]); 553 | axis("off"); 554 | pause(1.0/FPS); 555 | end 556 | else 557 | axis off; 558 | movie(figM,F); 559 | end 560 | [Xm,Ym,button] = ginput(1); 561 | end 562 | filename = sprintf('IK_Solver_%d.avi',IKmethod); 563 | filepath = sprintf('./images/%s',filename); 564 | fprintf('Recorded movie frames saved as file %s\n',filepath); 565 | if isOctave 566 | info = imfinfo(F(1,1:end)); 567 | image2avi(FMT, FPS, info.Width, info.Height, filepath(1:end)); 568 | else 569 | v = VideoWriter(filepath, 'Motion JPEG AVI'); 570 | v.FrameRate = FPS; 571 | open(v); 572 | writeVideo(v,F); 573 | close(v); 574 | end 575 | end 576 | 577 | fprintf('Program exit.\n'); 578 | --------------------------------------------------------------------------------