├── .gitignore ├── ForwardKin.m ├── FullDyn.m ├── GravityCompT1.m ├── GravityCompT2.m ├── ImpedenceControl.m ├── MAIN.m ├── Plotter.m ├── README.md ├── Thdotdot1.m ├── Thdotdot2.m └── deriverRelativeAngles.m /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled source # 2 | ################### 3 | *.com 4 | *.class 5 | *.dll 6 | *.exe 7 | *.o 8 | *.so 9 | 10 | # Packages # 11 | ############ 12 | # it's better to unpack these files and commit the raw source 13 | # git has its own built in compression methods 14 | *.7z 15 | *.dmg 16 | *.gz 17 | *.iso 18 | *.jar 19 | *.rar 20 | *.tar 21 | *.zip 22 | 23 | # Logs and databases # 24 | ###################### 25 | *.log 26 | *.sql 27 | *.sqlite 28 | 29 | # OS generated files # 30 | ###################### 31 | .DS_Store 32 | .DS_Store? 33 | ._* 34 | .Spotlight-V100 35 | .Trashes 36 | ehthumbs.db 37 | Thumbs.db 38 | 39 | # Other MATLAB 40 | *.m~ 41 | backup -------------------------------------------------------------------------------- /ForwardKin.m: -------------------------------------------------------------------------------- 1 | function ra_e = ForwardKin(l1,l2,th1,th2) 2 | %FORWARDKIN 3 | % RA_E = FORWARDKIN(L1,L2,TH1,TH2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 6.1. 6 | % 24-Nov-2014 12:33:25 7 | 8 | t2 = th1+th2; 9 | ra_e = [-l2.*sin(t2)-l1.*sin(th1);l2.*cos(t2)+l1.*cos(th1);0.0]; 10 | -------------------------------------------------------------------------------- /FullDyn.m: -------------------------------------------------------------------------------- 1 | function [zdot, T1, T2] = FullDyn( t,z,p ) 2 | %FULDYN Full dynamics for the double pendulum. Uses autogenerated Thdotdot1 3 | %and 2. 4 | th1 = z(1); 5 | th2 = z(3); 6 | thdot1 = z(2); 7 | thdot2 = z(4); 8 | % Fdx = 0; 9 | % Fdy = 0; 10 | 11 | %Current disturbance force on end effector 12 | FxCurrent = p.Fx; 13 | FyCurrent = p.Fy; 14 | 15 | %Current Target 16 | xCurrentTar = p.xtarget; 17 | yCurrentTar = p.ytarget; 18 | 19 | xdotCurrentTar = 0; 20 | ydotCurrentTar = 0; 21 | 22 | %Torque to track our desired point 23 | T = ImpedenceControl(p.Kd,p.Kp,p.l1,p.l2,th1,th2,thdot1,thdot2,xdotCurrentTar,xCurrentTar,ydotCurrentTar,yCurrentTar); 24 | 25 | %Add gravity compensation 26 | T1 = T(1) + GravityCompT1(0,0,p.d1,p.d2,p.g,p.l1,p.l2,p.m1,p.m2,th1,th2,thdot1,thdot2); 27 | T2 = T(2) + GravityCompT2(0,0,p.d2,p.g,p.l1,p.l2,p.m2,th1,th2,thdot1); 28 | 29 | %Use the autoderived functions for the accelerations. 30 | thdotdot1 = Thdotdot1(FxCurrent,FyCurrent,p.I1,p.I2,T1,T2,p.d1,p.d2,p.g,p.l1,p.l2,p.m1,p.m2,th1,th2,thdot1,thdot2); 31 | thdotdot2 = Thdotdot2(FxCurrent,FyCurrent,p.I1,p.I2,T1,T2,p.d1,p.d2,p.g,p.l1,p.l2,p.m1,p.m2,th1,th2,thdot1,thdot2); 32 | 33 | %Assemble the state vector derivatives. 34 | zdot = [thdot1 35 | thdotdot1 36 | thdot2 37 | thdotdot2 38 | ]; 39 | 40 | end 41 | 42 | -------------------------------------------------------------------------------- /GravityCompT1.m: -------------------------------------------------------------------------------- 1 | function T1Eq = GravityCompT1(Fdx,Fdy,d1,d2,g,l1,l2,m1,m2,th1,th2,thdot1,thdot2) 2 | %GRAVITYCOMPT1 3 | % T1EQ = GRAVITYCOMPT1(FDX,FDY,D1,D2,G,L1,L2,M1,M2,TH1,TH2,THDOT1,THDOT2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 6.1. 6 | % 24-Nov-2014 12:33:30 7 | 8 | t2 = th1+th2; 9 | t3 = sin(t2); 10 | t4 = sin(th1); 11 | t5 = sin(th2); 12 | T1Eq = Fdy.*l1.*t4+Fdy.*l2.*t3+Fdx.*l2.*cos(t2)+Fdx.*l1.*cos(th1)-d1.*g.*m1.*t4-d2.*g.*m2.*t3-g.*l1.*m2.*t4-d2.*l1.*m2.*t5.*thdot2.^2-d2.*l1.*m2.*t5.*thdot1.*thdot2.*2.0; 13 | -------------------------------------------------------------------------------- /GravityCompT2.m: -------------------------------------------------------------------------------- 1 | function T2Eq = GravityCompT2(Fdx,Fdy,d2,g,l1,l2,m2,th1,th2,thdot1) 2 | %GRAVITYCOMPT2 3 | % T2EQ = GRAVITYCOMPT2(FDX,FDY,D2,G,L1,L2,M2,TH1,TH2,THDOT1) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 6.1. 6 | % 24-Nov-2014 12:33:30 7 | 8 | t2 = th1+th2; 9 | t3 = sin(t2); 10 | T2Eq = Fdy.*l2.*t3+Fdx.*l2.*cos(t2)-d2.*g.*m2.*t3+d2.*l1.*m2.*thdot1.^2.*sin(th2); 11 | -------------------------------------------------------------------------------- /ImpedenceControl.m: -------------------------------------------------------------------------------- 1 | function Ta = ImpedenceControl(Kd,Kp,l1,l2,th1,th2,thdot1,thdot2,xdott,xt,ydott,yt) 2 | %IMPEDENCECONTROL 3 | % TA = IMPEDENCECONTROL(KD,KP,L1,L2,TH1,TH2,THDOT1,THDOT2,XDOTT,XT,YDOTT,YT) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 6.1. 6 | % 24-Nov-2014 12:33:31 7 | 8 | t2 = th1+th2; 9 | t3 = sin(t2); 10 | t4 = l2.*t3; 11 | t5 = sin(th1); 12 | t6 = l1.*t5; 13 | t7 = t4+t6; 14 | t8 = cos(t2); 15 | t9 = l2.*t8; 16 | t10 = cos(th1); 17 | t11 = l1.*t10; 18 | t12 = t9+t11; 19 | t13 = t12.*thdot1; 20 | t14 = l2.*t8.*thdot2; 21 | t15 = t13+t14+xdott; 22 | t16 = Kd.*t15; 23 | t17 = t4+t6+xt; 24 | t18 = Kp.*t17; 25 | t19 = t16+t18; 26 | t20 = t7.*thdot1; 27 | t21 = l2.*t3.*thdot2; 28 | t22 = t20+t21+ydott; 29 | t23 = Kd.*t22; 30 | t24 = t9+t11-yt; 31 | t25 = t23-Kp.*t24; 32 | Ta = [-t12.*t19-t7.*t25;-l2.*t8.*t19-l2.*t3.*t25]; 33 | -------------------------------------------------------------------------------- /MAIN.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Two link robot arm with control to track a point that the user clicks. 3 | % 4 | % Files: 5 | % MAIN - Execute this file; parameters here. 6 | % 7 | % Plotter -- Handles all integration and graphics (since the two are linked 8 | % in the live-integration version) 9 | % 10 | % FullDyn -- Dynamics function of the form zdot = dynamics(z,t,params). 11 | % This function evaluates BOTH the controller and the dynamics. Can 12 | % probably be crunched through ode45 if you don't care about user 13 | % interaction. 14 | % 15 | % deriverRelativeAngles -- does symbolic algebra to derive the dynamics and 16 | % the control equations. Automatically writes these to MATLAB functions: 17 | % - ForwardKin, GravityCompT1, GravityCompT2, ImpedanceControl, 18 | % Thdotdot1, Thdotdot2 --- All these are autogenerated and should not be 19 | % directly altered. 20 | % 21 | % 22 | % Matthew Sheen, 2014 23 | % 24 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 25 | clc; close all; 26 | 27 | clear all; 28 | 29 | rederive = false; 30 | %%%%%%%% System Parameters %%%%%%%% 31 | 32 | %Initial conditions: 33 | p.init = [pi/4 0.0 pi/4 0.0]'; 34 | 35 | p.g = 9.81; 36 | p.m1 = 1; %Mass of link 1. 37 | p.m2 = 1; %Mass of link 2. 38 | p.l1 = 1; %Total length of link 1. 39 | p.l2 = 1; %Total length of link 2. 40 | p.d1 = p.l1/2; %Center of mass distance along link 1 from the fixed joint. 41 | p.d2 = p.l2/2; %Center of mass distance along link 2 from the fixed joint. 42 | p.I1 = 1/12*p.m1*p.l1^2; %Moment of inertia of link 1 about COM 43 | p.I2 = 1/12*p.m2*p.l2^2; %Moment of inertia of link 2 about COM 44 | 45 | endZ = ForwardKin(p.l1,p.l2,p.init(1),p.init(3)); 46 | x0 = endZ(1); %End effector initial position in world frame. 47 | y0 = endZ(2); 48 | p.Fx = 0; 49 | p.Fy = 0; 50 | 51 | %%%%%%%% Control Parameters %%%%%%%% 52 | 53 | %Controller Gains 54 | p.Kp = 10; 55 | p.Kd = 8; 56 | 57 | %Single target: 58 | p.xtarget = x0; %What points are we shooting for in WORLD SPACE? 59 | p.ytarget = y0; 60 | 61 | %%%%%%%% Run Derivers %%%%%%%% 62 | 63 | if rederive 64 | %If the gain matrix hasn't been found yet, then we assume derivation hasn't 65 | %happened yet. 66 | deriverRelativeAngles; 67 | disp('Equations of motion and control parameters derived using relative angles.'); 68 | end 69 | 70 | %%%%%%%% Integrate %%%%%%%% 71 | 72 | Plotter(p) %Integration is done in real time using symplectic euler like we did in the CS animation class. 73 | 74 | 75 | -------------------------------------------------------------------------------- /Plotter.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Animate the acrobot after the MAIN script has been run. 3 | % 4 | % Matthew Sheen, 2014 5 | % 6 | % Note: data is passed from this to the callback functions in the figure 7 | % object's UserData field. 8 | % For compatibility with 2014a and earlier, I use set/get instead of the 9 | % object.field notation. 10 | % Can also be done with global vars as in the backup version. I hate 11 | % global variables so, this version is the result. 12 | % 13 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 14 | function Plotter(p) 15 | close all 16 | 17 | %Playback speed: 18 | % playback = p.animationSpeed; 19 | 20 | %Name the whole window and define the mouse callback function 21 | f = figure; 22 | set(f,'WindowButtonMotionFcn','','WindowButtonDownFcn',@ClickDown,'WindowButtonUpFcn',@ClickUp,'KeyPressFc',@KeyPress); 23 | 24 | figData.xtarget = []; 25 | figData.ytarget = []; 26 | figData.Fx = []; 27 | figData.Fy = []; 28 | figData.xend = []; 29 | figData.yend = []; 30 | figData.fig = f; 31 | figData.tarControl = true; 32 | 33 | %%%%%%%% 1st Subplot -- the pendulum animation %%%%%%% 34 | figData.simArea = subplot(1,1,1); %Eliminated other subplots, but left this for syntax consistency. 35 | axis equal 36 | hold on 37 | 38 | %Create pendulum link1 object: 39 | width1 = p.l1*0.05; 40 | xdat1 = 0.5*width1*[-1 1 1 -1]; 41 | ydat1 = p.l1*[0 0 1 1]; 42 | link1 = patch(xdat1,ydat1, [0 0 0 0],'r'); 43 | 44 | %Create pendulum link2 object: 45 | width2 = p.l2*0.05; 46 | xdat2 = 0.5*width2*[-1 1 1 -1]; 47 | ydat2 = p.l2*[0 0 1 1]; 48 | link2 = patch(xdat2,ydat2, [0 0 0 0],'b'); 49 | axis([-3.5 3.5 -3.6 3.6]); 50 | 51 | %Dots for the hinges: 52 | h1 = plot(0,0,'.k','MarkerSize',40); %First link anchor 53 | h2 = plot(0,0,'.k','MarkerSize',40); %link1 -> link2 hinge 54 | 55 | %Timer label: 56 | timer = text(-3.2,-3.2,'0.00','FontSize',28); 57 | 58 | %Torque meters on screen 59 | tmeter1 = text(0.6,-3.2,'0.00','FontSize',22,'Color', 'r'); 60 | tmeter2 = text(2.2,-3.2,'0.00','FontSize',22,'Color', 'b'); 61 | 62 | %Target Pt. 63 | targetPt = plot(p.xtarget,p.ytarget,'xr','MarkerSize',30); 64 | 65 | hold off 66 | 67 | %Make the whole window big for handy viewing: 68 | set(f, 'units', 'inches', 'position', [5 5 10 9]) 69 | set(f,'Color',[1,1,1]); 70 | 71 | % Turn the axis off 72 | ax = get(f,'Children'); 73 | set(ax,'Visible','off'); 74 | 75 | %Animation plot loop -- Includes symplectic integration now. 76 | z1 = p.init; 77 | told = 0; 78 | 79 | set(f,'UserData',figData); 80 | 81 | tic %Start the clock 82 | while (ishandle(f)) 83 | figData = get(f,'UserData'); 84 | %%%% INTEGRATION %%%% 85 | tnew = toc; 86 | dt = tnew - told; 87 | 88 | %Old velocity and position 89 | xold = [z1(1),z1(3)]; 90 | vold = [z1(2),z1(4)]; 91 | 92 | %Call RHS given old state 93 | [zdot1, T1, T2] = FullDyn(tnew,z1,p); 94 | vinter1 = [zdot1(1),zdot1(3)]; 95 | ainter = [zdot1(2),zdot1(4)]; 96 | 97 | vinter2 = vold + ainter*dt; %Update velocity based on old RHS call 98 | 99 | %Update position. 100 | xnew = xold + vinter2*dt; 101 | vnew = (xnew-xold)/dt; 102 | 103 | z2 = [xnew(1) vnew(1) xnew(2) vnew(2)]; 104 | 105 | z1 = z2; 106 | told = tnew; 107 | %%%%%%%%%%%%%%%%%%%% 108 | 109 | %If there are new mouse click locations, then set those as the new 110 | %target. 111 | if ~isempty(figData.xtarget) 112 | p.xtarget = figData.xtarget; 113 | end 114 | 115 | if ~isempty(figData.ytarget) 116 | p.ytarget = figData.ytarget; 117 | end 118 | set(targetPt,'xData',p.xtarget); %Change the target point graphically. 119 | set(targetPt,'yData',p.ytarget); 120 | 121 | %When you hit a key, it changes to force mode, where the mouse will 122 | %pull things. 123 | ra_e = ForwardKin(p.l1,p.l2,z1(1),z1(3)); 124 | figData.xend = ra_e(1); 125 | figData.yend = ra_e(2); 126 | set(f,'UserData',figData); 127 | 128 | if ~isempty(figData.Fx) 129 | p.Fx = figData.Fx; 130 | end 131 | if ~isempty(figData.Fy) 132 | p.Fy = figData.Fy; 133 | end 134 | 135 | tstar = told; %Get the time (used during this entire iteration) 136 | 137 | %On screen timer. 138 | set(timer,'string',strcat(num2str(tstar,3),'s')) 139 | zstar = z1;%interp1(time,zarray,tstar); %Interpolate data at this instant in time. 140 | 141 | %Rotation matrices to manipulate the vertices of the patch objects 142 | %using theta1 and theta2 from the output state vector. 143 | rot1 = [cos(zstar(1)), -sin(zstar(1)); sin(zstar(1)),cos(zstar(1))]*[xdat1;ydat1]; 144 | set(link1,'xData',rot1(1,:)) 145 | set(link1,'yData',rot1(2,:)) 146 | 147 | rot2 = [cos(zstar(3)+zstar(1)), -sin(zstar(3)+zstar(1)); sin(zstar(3)+zstar(1)),cos(zstar(3)+zstar(1))]*[xdat2;ydat2]; 148 | 149 | set(link2,'xData',rot2(1,:)+(rot1(1,3)+rot1(1,4))/2) %We want to add the midpoint of the far edge of the first link to all points in link 2. 150 | set(link2,'yData',rot2(2,:)+(rot1(2,3)+rot1(2,4))/2) 151 | 152 | %Change the hinge dot location 153 | set(h2,'xData',(rot1(1,3)+rot1(1,4))/2) 154 | set(h2,'yData',(rot1(2,3)+rot1(2,4))/2) 155 | 156 | %Show torques on screen (text only atm) update for time series later. 157 | set(tmeter1,'string',strcat(num2str(T1,2),' Nm')); 158 | set(tmeter2,'string',strcat(num2str(T2,2),' Nm')); 159 | 160 | drawnow; 161 | end 162 | end 163 | 164 | %%%% BEGIN CALLBACKS FOR MOUSE AND KEYBOARD STUFF %%%%% 165 | 166 | % When click-up occurs, disable the mouse motion detecting callback 167 | function ClickUp(varargin) 168 | figData = get(varargin{1},'UserData'); 169 | set(figData.fig,'WindowButtonMotionFcn',''); 170 | figData.Fx = 0; 171 | figData.Fy = 0; 172 | set(varargin{1},'UserData',figData); 173 | end 174 | 175 | % When click-down occurs, enable the mouse motion detecting callback 176 | function ClickDown(varargin) 177 | figData = get(varargin{1},'UserData'); 178 | figData.Fx = 0; 179 | figData.Fy = 0; 180 | 181 | set(figData.fig,'WindowButtonMotionFcn',@MousePos); 182 | set(varargin{1},'UserData',figData); 183 | end 184 | 185 | % any keypress switches from dragging the setpoint to applying a 186 | % disturbance. 187 | function KeyPress(hObject, eventdata, handles) 188 | 189 | figData = get(hObject,'UserData'); 190 | 191 | figData.tarControl = ~figData.tarControl; 192 | 193 | if figData.tarControl 194 | disp('Mouse will change the target point of the end effector.') 195 | else 196 | disp('Mouse will apply a force on end effector.') 197 | end 198 | set(hObject,'UserData',figData); 199 | end 200 | 201 | % Checks mouse position and sends it back up. 202 | function MousePos(varargin) 203 | figData = get(varargin{1},'UserData'); 204 | 205 | mousePos = get(figData.simArea,'CurrentPoint'); 206 | if figData.tarControl 207 | figData.xtarget = mousePos(1,1); 208 | figData.ytarget = mousePos(1,2); 209 | else 210 | figData.Fx = 10*(mousePos(1,1)-figData.xend); 211 | figData.Fy = 10*(mousePos(1,2)-figData.yend); 212 | end 213 | set(varargin{1},'UserData',figData); 214 | end -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MATLAB Impedance Control for a 2-Link Robot Arm - User-interactive 2 | 2-link planar arm with a compliant controller and gravity compensation. The user can click and drag to move the end-effector's target position. Hit a keyboard key to change to "disturbance-mode." This mode instead applies a disturbance force to the end-effector. 3 | 4 | ## Files 5 | MAIN.m -- Run this first! 6 | Plotter.m -- Display the arm and integrate the equations in real time 7 | deriverRelativeAngles.m -- Derives all dynamics and controls equations. 8 | ALL other files -- Auto-written by deriverRelativeAngles.m 9 | 10 | ## Video 11 | https://www.youtube.com/watch?v=cmW7pRLut8A -------------------------------------------------------------------------------- /Thdotdot1.m: -------------------------------------------------------------------------------- 1 | function eqn1 = Thdotdot1(Fdx,Fdy,I1,I2,T1,T2,d1,d2,g,l1,l2,m1,m2,th1,th2,thdot1,thdot2) 2 | %THDOTDOT1 3 | % EQN1 = THDOTDOT1(FDX,FDY,I1,I2,T1,T2,D1,D2,G,L1,L2,M1,M2,TH1,TH2,THDOT1,THDOT2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 6.1. 6 | % 24-Nov-2014 12:33:28 7 | 8 | t2 = d2.^2; 9 | t3 = l1.^2; 10 | t4 = d1.^2; 11 | t5 = m2.^2; 12 | t6 = sin(th2); 13 | t7 = cos(th1); 14 | t8 = sin(th1); 15 | t9 = thdot1.^2; 16 | t10 = th2.*2.0; 17 | t11 = t10+th1; 18 | t12 = sin(t11); 19 | t13 = thdot2.^2; 20 | eqn1 = (I2.*T1.*2.0-I2.*T2.*2.0+T1.*m2.*t2.*2.0-T2.*m2.*t2.*2.0-Fdx.*I2.*l1.*t7.*2.0-Fdy.*I2.*l1.*t8.*2.0+t2.*t3.*t5.*t9.*sin(t10)+I2.*d1.*g.*m1.*t8.*2.0+I2.*g.*l1.*m2.*t8.*2.0-Fdx.*l1.*m2.*t2.*t7.*2.0-Fdy.*l1.*m2.*t2.*t8.*2.0+g.*l1.*t2.*t5.*t8-g.*l1.*t2.*t5.*t12-T2.*d2.*l1.*m2.*cos(th2).*2.0+Fdx.*d2.*l1.*l2.*m2.*cos(t11)+Fdx.*d2.*l1.*l2.*m2.*t7+Fdy.*d2.*l1.*l2.*m2.*t8+Fdy.*d2.*l1.*l2.*m2.*t12+I2.*d2.*l1.*m2.*t6.*t9.*2.0+I2.*d2.*l1.*m2.*t6.*t13.*2.0+d1.*g.*m1.*m2.*t2.*t8.*2.0+d2.*l1.*t2.*t5.*t6.*t9.*2.0+d2.*l1.*t2.*t5.*t6.*t13.*2.0+I2.*d2.*l1.*m2.*t6.*thdot1.*thdot2.*4.0+d2.*l1.*t2.*t5.*t6.*thdot1.*thdot2.*4.0)./(I1.*I2.*2.0+I1.*m2.*t2.*2.0+I2.*m1.*t4.*2.0+I2.*m2.*t3.*2.0+t2.*t3.*t5+m1.*m2.*t2.*t4.*2.0-t2.*t3.*t5.*cos(t10)); 21 | -------------------------------------------------------------------------------- /Thdotdot2.m: -------------------------------------------------------------------------------- 1 | function eqn2 = Thdotdot2(Fdx,Fdy,I1,I2,T1,T2,d1,d2,g,l1,l2,m1,m2,th1,th2,thdot1,thdot2) 2 | %THDOTDOT2 3 | % EQN2 = THDOTDOT2(FDX,FDY,I1,I2,T1,T2,D1,D2,G,L1,L2,M1,M2,TH1,TH2,THDOT1,THDOT2) 4 | 5 | % This function was generated by the Symbolic Math Toolbox version 6.1. 6 | % 24-Nov-2014 12:33:29 7 | 8 | t2 = d2.^2; 9 | t3 = l1.^2; 10 | t4 = d1.^2; 11 | t5 = m2.^2; 12 | t6 = th1+th2; 13 | t7 = sin(t6); 14 | t8 = thdot1.^2; 15 | t9 = sin(th2); 16 | t10 = cos(th1); 17 | t11 = sin(th1); 18 | t12 = th1-th2; 19 | t13 = cos(th2); 20 | t14 = th2.*2.0; 21 | t15 = thdot2.^2; 22 | t16 = sin(t14); 23 | t17 = sin(t12); 24 | t18 = cos(t6); 25 | t19 = t14+th1; 26 | t20 = sin(t19); 27 | eqn2 = -(I1.*T2.*-2.0+I2.*T1.*2.0-I2.*T2.*2.0+T1.*m2.*t2.*2.0-T2.*m2.*t2.*2.0-T2.*m1.*t4.*2.0-T2.*m2.*t3.*2.0-Fdx.*I2.*l1.*t10.*2.0+Fdx.*I1.*l2.*t18.*2.0+Fdy.*I1.*l2.*t7.*2.0-Fdy.*I2.*l1.*t11.*2.0-I1.*d2.*g.*m2.*t7.*2.0+I2.*d1.*g.*m1.*t11.*2.0+I2.*g.*l1.*m2.*t11.*2.0-Fdx.*d2.*m2.*t3.*t18-Fdy.*d2.*m2.*t3.*t7-Fdy.*d2.*m2.*t3.*t17+T1.*d2.*l1.*m2.*t13.*2.0-T2.*d2.*l1.*m2.*t13.*4.0-Fdx.*l1.*m2.*t2.*t10.*2.0+Fdx.*l2.*m1.*t4.*t18.*2.0+Fdx.*l2.*m2.*t3.*t18.*2.0+Fdy.*l2.*m1.*t4.*t7.*2.0+Fdy.*l2.*m2.*t3.*t7.*2.0-Fdy.*l1.*m2.*t2.*t11.*2.0-d2.*g.*t3.*t5.*t7+d2.*g.*t3.*t5.*t17+g.*l1.*t2.*t5.*t11-g.*l1.*t2.*t5.*t20+t2.*t3.*t5.*t8.*t16.*2.0+t2.*t3.*t5.*t15.*t16-Fdx.*d2.*m2.*t3.*cos(t12)+t2.*t3.*t5.*t16.*thdot1.*thdot2.*2.0+Fdx.*d2.*l1.*l2.*m2.*cos(t19)+Fdx.*d2.*l1.*l2.*m2.*t10+Fdy.*d2.*l1.*l2.*m2.*t11+Fdy.*d2.*l1.*l2.*m2.*t20+I1.*d2.*l1.*m2.*t8.*t9.*2.0+I2.*d2.*l1.*m2.*t8.*t9.*2.0+I2.*d2.*l1.*m2.*t9.*t15.*2.0-d2.*g.*m1.*m2.*t4.*t7.*2.0+d1.*g.*m1.*m2.*t2.*t11.*2.0+d2.*l1.*t2.*t5.*t8.*t9.*2.0+d2.*l1.*t3.*t5.*t8.*t9.*2.0+d2.*l1.*t2.*t5.*t9.*t15.*2.0+I2.*d2.*l1.*m2.*t9.*thdot1.*thdot2.*4.0+d1.*d2.*g.*l1.*m1.*m2.*t7+d1.*d2.*g.*l1.*m1.*m2.*t17+d2.*l1.*m1.*m2.*t4.*t8.*t9.*2.0+d2.*l1.*t2.*t5.*t9.*thdot1.*thdot2.*4.0)./(I1.*I2.*2.0+I1.*m2.*t2.*2.0+I2.*m1.*t4.*2.0+I2.*m2.*t3.*2.0+t2.*t3.*t5+m1.*m2.*t2.*t4.*2.0-t2.*t3.*t5.*cos(t14)); 28 | -------------------------------------------------------------------------------- /deriverRelativeAngles.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %%%%%%%%%%%%%%%% Derive double pendulum dynamics %%%%%%%%%%%%%%%% 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | 5 | %Derive double pendulum equations of motion, write to Thdotdot1 and 6 | %Thdotdot2 matlab function files. THIS VERSION DERIVES IT WITH THE ELBOW 7 | %ANGLE RELATIVE TO THE UPPER ARM ANGLE. 8 | 9 | %Parameters symbolically. 10 | syms m1 m2 I1 I2 g l1 l2 d1 d2 th1 th2 thdot1 thdot2 thdotdot1 thdotdot2 er1 er2 eth1 eth2 T1 T2 real 11 | 12 | %Unit vectors, cartesian 13 | i = [1 0 0]'; 14 | j = [0 1 0]'; 15 | k = [0 0 1]'; 16 | 17 | %Rotating reference frames 18 | er1 = [-sin(th1), cos(th1), 0]'; %Just changed it so 0 is upright. 19 | er2 = [-sin(th2+th1), cos(th2+th1), 0]'; 20 | 21 | eth1 = [-cos(th1),-sin(th1),0]'; 22 | eth2 = [-cos(th2+th1),-sin(th2+th1),0]'; 23 | 24 | %Vectors to significant points 25 | ra_c1 = d1*er1; %A is fixed point, B is the elbow, c1 and c2 are COMs, e end effector 26 | rb_c2 = d2*er2; 27 | rb_e = l2*er2; 28 | ra_b = l1*er1; 29 | ra_c2 = ra_b + rb_c2; 30 | ra_e = ra_b + rb_e; 31 | 32 | matlabFunction(ra_e, 'file', 'ForwardKin'); 33 | 34 | %Velocities 35 | Vc1 = d1*thdot1*eth1; 36 | VB = l1*thdot1*eth1; 37 | Vc2 = VB + d2*(thdot2+thdot1)*eth2; 38 | Ve = VB + l2*(thdot2+thdot1)*eth2; 39 | 40 | %Accelerations 41 | Ac1 = d1*thdotdot1*eth1 - d1*thdot1^2*er1; 42 | AB = l1*thdotdot1*eth1 - l1*thdot1^2*er1; 43 | Ac2 = d2*(thdotdot2+thdotdot1)*eth2 - d2*(thdot1 + thdot2)^2*er2 + AB; 44 | 45 | %Force at end effector 46 | syms Fdx Fdy real 47 | Fd = [Fdx, Fdy, 0]'; 48 | 49 | %AMB for just link 2: 50 | M_B = cross(rb_c2,-m2*g*j)+T2*k + cross(rb_e,Fd); %Last term is force at end effector. 51 | Hdot2 = I2*(thdotdot2+thdotdot1)*k + cross(rb_c2, m2*Ac2); 52 | 53 | eqn2forthdotdot2 = solve(dot(Hdot2 - M_B,k),thdotdot2); 54 | 55 | %AMB for whole thing: 56 | M_A = cross(ra_c2,-m2*g*j) + cross(ra_c1,-m1*g*j) + T1*k + cross(ra_e,Fd); %Gravity for both, plus a control torque. Last term is force at end effector 57 | Hdot1 = I2*(thdotdot2+thdotdot1)*k + cross(ra_c2, m2*Ac2) + I1*thdotdot1*k + cross(ra_c1, m1*Ac1); 58 | 59 | eqn1forthdotdot1 = solve(dot(Hdot1 - M_A,k),thdotdot1); 60 | 61 | %One equation for thdotdot1, one for thdotdot2. 62 | eqn2 = simplify(solve(subs(eqn2forthdotdot2, thdotdot1, eqn1forthdotdot1)-thdotdot2,thdotdot2)); 63 | eqn1 = simplify(solve(subs(eqn1forthdotdot1, thdotdot2, eqn2forthdotdot2)-thdotdot1,thdotdot1)); 64 | 65 | %Create matlab functions for thdotdot1 and 2: 66 | matlabFunction(eqn1, 'file', 'Thdotdot1'); 67 | matlabFunction(eqn2, 'file', 'Thdotdot2'); 68 | 69 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 70 | %%%%%%%%%%%%%%%% Gravity Compensation %%%%%%%%%%%%%%%%%%%%%%%%%%% 71 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 72 | 73 | T2Eq = simplify(solve((solve(eqn1,T1)-solve(eqn2,T1)),T2)); 74 | 75 | T1Eq = simplify(subs(solve(eqn1,T1),T2,T2Eq)); 76 | 77 | matlabFunction(T1Eq, 'file', 'GravityCompT1'); 78 | matlabFunction(T2Eq, 'file', 'GravityCompT2'); 79 | 80 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 81 | %%%%%%%%%%%%%%%% Impedance control? %%%%%%%%%%%%%%%%%%%%%%%%%%% 82 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 83 | 84 | %Jacobian relating end effector velocity to joint space vel 85 | % ie Ve = J*qv 86 | 87 | J = jacobian(Ve,{thdot1 thdot2}); 88 | 89 | syms Kp Kd xt yt xdott ydott real 90 | 91 | zt = [xt yt 0 ]'; %Trajectory tracked 92 | ztdot = [xdott ydott 0]'; %velocity tracked 93 | 94 | Ta = J'*(Kp*(zt - ra_e) + Kd*(ztdot - J*[thdot1 thdot2]')); 95 | 96 | matlabFunction(Ta, 'file', 'ImpedenceControl'); 97 | 98 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 99 | %%%%%%%%%%%%%%%% Energy eqns %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 100 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 101 | 102 | %Turned this off after making sure stuff worked: 103 | 104 | % syms k1 k2 real 105 | % 106 | % PE = g*dot(ra_c2,j)*m2 + g*dot(ra_c1,j)*m1; 107 | % KE = 1/2*I1*thdot1^2 + 1/2*I2*(thdot2+thdot1)^2 + 1/2*m1*dot(Vc1,Vc1) + 1/2*m2*dot(Vc2,Vc2); 108 | % 109 | % Etot = PE + KE; 110 | % 111 | % matlabFunction(Etot, 'file', 'TotEnergy'); 112 | 113 | --------------------------------------------------------------------------------