MMAE-540 Adv. Robotics and Mechatronics - Fall 2007 Homework 4

Transcription

1 MMAE-54 Adv. Robotics and Mechatronics - Fall 27 Homework 4 Assigned Wednesday Sep. 9th Due Wednesday Sept. 26th.5 Trajectory for t = s.5 Trajectory for t = 2 s Trajectory for t = 5 s.5 Trajectory for t = s Figure : End effector trajectories for manipulator arm - no external torque.. Simulate the 2-link Manipulator. (a) We want to use Matlab to simulate the above ordinary differential equation. What functions does Matlab provide to solve ODE s? Matlab uses a variety of ODE functions including ODE45, ODE23, ODE3, etc. (b) What is the difference between the functions? The differences lie the accuracy of the solver, the time it takes the solver to solve the problem, and what types of systems the solver is good at solving. For example, ODE5s, ODE23s, ODE23tb are good at solving stiff systems. Some numerical solvers can only solve stiff problems if the time step is taken to be incredibly small. If you find that the solution to your simulation is always failing, but you are sure that your code is correct, you might want to try one of the stiff solvers. Note that Wikipedia actually has a decent entry on stiff ordinary differential equations. (c) Simulate the trajectory of the end effector in coordinates at time t =, 2, 5, s. Use the subplot command to plot the four graphs in a 2x2 array. Attach your code. See Figure. 2. Given the same manipulator arm and initial conditions, assume a torque is applied to the joints: [ ] e t T = Simulate the trajectory of the end effector in coordinates at time t =, 2, 5, s. Use the subplot command to plot the four graphs in a 2x2 array. Attach your code. See Figure Given the same manipulator and initial conditions, you will simulate and comment on the performance of a PD joint controller moving from q i = [ ] T to qf = [ 45 ] T when: gravity = (e.g. the manipulator is in the horizontal plane). (a) Pick K p and K d to produce reasonable results. What are those values? Picking K p = 5 and K d = 5 yielded reasonable results. Other values could obviously be used.

2 .5 Trajectory for t = s.5 Trajectory for t = 2 s Trajectory for t = 5 s Trajectory for t = s Figure 2: End effector trajectories for manipulator arm - external torque applied. Joint Angle, deg Joint Velocity, deg/s Joint 2 Angle, deg time, s Joint 2 Velocity, deg/s time, s Figure 3: Joint angles and velocities for Joint PD Control Law. 2

3 Figure 4: End effector trajectory for Joint PD Control Law. Joint Angle, rad.5.5 Joint Velocity, rad/s Joint 2 Angle, rad Joint 2 Velocity, rad/s time, s time, s Figure 5: Joint positions and velocities for the end effector PD control law 3

4 Figure 6: End effector position for the end effector PD control law (b) Does you system approximate a linear system? The system approximates a damped second order system in this case. (c) What is the settling time of the joint positions and velocities? Both joint positions and joint velocities settle down after about six seconds. (d) Is there any overshoot? There is a small amount of overshoot in joint. (e) Are there any parts of the trajectory that do not approximate a linear system? Why do you think this is the case? Joint 2 has an interesting hitch around the peak time of joint - presumably this is due to the dynamics of joint affecting joint 2 at this time. (f) What is the steady-state error for the position of each of the joints. A degree at most. (g) Plot the joint angles (deg) and velocities (deg/s) as a function of time for s. If the response does not reach a steady state before s then adjust K p and K d. Use the subplot function to produce a 2x2 array of the plots. See Figure 3. (h) Plot the end point trajectory in and coordinates. Label the start and finish. See Figure 4. (i) How would the control law change when: gravity = g = 9.8m / s 2 (e.g. the manipulator is in the vertical plane)? The control law should now be: u = g (q) K P q K D q 4. Now use an endpoint PD control law instead of a joint PD control law. (a) What is the endpoint control law? The control law should be: u = g (q) J T (K P x + K D ẋ) (b) Plot the joint angles (deg) and velocities (deg/s) as a function of time for s. Use the subplot function to produce a 2x2 array of the plots. See Figure 5. 4

5 (c) Plot the end point trajectory in and coordinates. Label the start and finish. See Figure 6. (d) Compare and contrast the endpoint control law with the joint control law. The system response no longer looks like a second order damped system for the joints, but it still converges to the correct point. 5

6 9/23/9 :43 PM C:\Users\Matthew Spenko\Documents\Classes\MMAE...\Homework_3_2.m of %use the inline function: %[T,] = ode45('homework_3_2', [ 2], [3*pi/8; ; 6*pi/8; ]) %to simulate the system using Matlabs ODE solver function answer = Homework_3_2(t,x) %read in the variables q = x(); qdot = x(2); q2 = x(3); q2dot = x(4); %set the manipulator constants m = 5; m2 = 5; g = 9.8; L = ; L2 = ; Lc = L/2; Lc2 = L2/2; I = m*l^2/2; I2 = m2*l2^2/2; H = I + I2 + m*lc^2 + m2*(l^2 + Lc2^2 + 2*L*Lc2*cos(q2)); H22 = m2*lc2^2 + I2; H2 = m2*(lc2^2 + L*Lc2*cos(q2)) + I2; H2 = H2; h = m2*l*lc2*sin(q2); g = (m*lc + m2*l)*g*cos(q) + m2*lc2*g*cos(q + q2); g2 = m2*lc2*g*cos(q + q2); H = [ H, H2;... H2, H22]; G = [g; g2]; C = [-h*q2dot, -h*qdot - h*q2dot;... h*qdot, ]; qddot = inv(h)*(-g-c*[qdot; q2dot]); %compute the acceleration answer = [qdot; qddot(); q2dot; qddot(2)];

7 9/23/9 :43 PM C:\Users\Matthew Spenko\Documents\Classes\...\Homework_3_2_plot.m of clear for i = :4 timefinal = [ 2 5 ]; [T,] = ode45('homework_3_2', [ timefinal(i)], [3*pi/8; ; 6*pi/8; ]); end %T is the time vector % is the vector of q, qdot, q2, q2dot q = (:,); qdot = (:,2); q2 = (:,3); q2dot = (:,4); %Transform the joint angles into end effector positions L = ; L2 = ; x = L*cos(q) + L2*cos(q + q2); y = L*sin(q) + L2*sin(q + q2); figure() subplot(2,2,i) plot(x,y,'linewidth',2) xlabel('') ylabel('') title(['trajectory for t = ' num2str(timefinal(i)) ' s']) axis equal grid on

8 9/23/9 :43 PM C:\Users\Matthew Spenko\Documents\Classes\MMAE...\Homework_3_3.m of %use the inline function: %[T,] = ode45('homework_3_2', [ 2], [3*pi/8; ; 6*pi/8; ]) %to simulate the system using Matlabs ODE solver function answer = Homework_3_2(t,x) %read in the variables q = x(); qdot = x(2); q2 = x(3); q2dot = x(4); %set the manipulator constants m = 5; m2 = 5; g = 9.8; L = ; L2 = ; Lc = L/2; Lc2 = L2/2; I = m*l^2/2; I2 = m2*l2^2/2; H = I + I2 + m*lc^2 + m2*(l^2 + Lc2^2 + 2*L*Lc2*cos(q2)); H22 = m2*lc2^2 + I2; H2 = m2*(lc2^2 + L*Lc2*cos(q2)) + I2; H2 = H2; h = m2*l*lc2*sin(q2); g = (m*lc + m2*l)*g*cos(q) + m2*lc2*g*cos(q + q2); g2 = m2*lc2*g*cos(q + q2); H = [ H, H2;... H2, H22]; %set the G = [g; g2]; C = [-h*q2dot, -h*qdot - h*q2dot;... h*qdot, ]; T = [-*exp(-t); ]; qddot = inv(h)*(t-g-c*[qdot; q2dot]); %compute the acceleration answer = [qdot; qddot(); q2dot; qddot(2)];

9 9/23/9 :44 PM C:\Users\Matthew Spenko\Documents\Classes\MMAE...\Homework_4_.m of function answer = Homework_4_(t,x) %read in the variables q = x(); qdot = x(2); q2 = x(3); q2dot = x(4); %set the manipulator constants m = 5; m2 = 5; g = 9.8; L = ; L2 = ; Lc = L/2; Lc2 = L2/2; I = m*l^2/2; I2 = m2*l2^2/2; %Calculate the matrix components H = I + I2 + m*lc^2 + m2*(l^2 + Lc2^2 + 2*L*Lc2*cos(q2)); H22 = m2*lc2^2 + I2; H2 = m2*(lc2^2 + L*Lc2*cos(q2)) + I2; H2 = H2; h = m2*l*lc2*sin(q2); g = (m*lc + m2*l)*g*cos(q) + m2*lc2*g*cos(q + q2); g2 = m2*lc2*g*cos(q + q2); H = [ H, H2;... H2, H22]; G = [g; g2]; C = [-h*q2dot, -h*qdot - h*q2dot;... h*qdot, ]; %Define the parameters for the control laws Q = [q;q2]; Qdot = [qdot; q2dot]; Qdesired = [45*pi/8; *pi/8]; Qtilda = Q - Qdesired; %set the joint torques Kp = 5; Kd = 5; Torque = -Kp*Qtilda- Kd*Qdot; % qddot = inv(h)*(torque-g-c*[qdot; q2dot]); %compute the acceleration with gravity qddot = inv(h)*(torque-c*qdot); %compute the acceleration without gravity answer = [qdot; qddot(); q2dot; qddot(2)];

10 9/23/9 :44 PM C:\Users\Matthew Spenko\Documents\Classes\MMAE...\Homework_4_2.m of 2 %use the inline function: %[T,] = ode45('homework_3_2', [ 2], [3*pi/8; ; 6*pi/8; ]) %to simulate the system using Matlabs ODE solver function answer = Homework_4_(t,x) %read in the variables q = x(); qdot = x(2); q2 = x(3); q2dot = x(4); %set the manipulator constants m = ; m2 = ; g = 9.8; L = ; L2 = ; Lc = L/2; Lc2 = L2/2; I = m*l^2/2; I2 = m2*l2^2/2; H = I + I2 + m*lc^2 + m2*(l^2 + Lc2^2 + 2*L*Lc2*cos(q2)); H22 = m2*lc2^2 + I2; H2 = m2*(lc2^2 + L*Lc2*cos(q2)) + I2; H2 = H2; h = m2*l*lc2*sin(q2); g = (m*lc + m2*l)*g*cos(q) + m2*lc2*g*cos(q + q2); g2 = m2*lc2*g*cos(q + q2); H = [ H, H2;... H2, H22]; %set the G = [g; g2]; C = [-h*q2dot, -h*qdot - h*q2dot;... h*qdot, ]; Q = [q;q2]; Qdot = [qdot; q2dot]; Qdesired = [45*pi/8; *pi/8]; Qtilda = Qdesired - Q; J = [ -L*sin(q)-L2*sin(q+q2), -L2*sin(q+q2);... L*cos(q)+L2*cos(q+q), L2*cos(q+q2)]; dot = J*Qdot; desired = [L*cos(Qdesired()) + L2*cos(Qdesired() + Qdesired(2));... L*sin(Qdesired()) + L2*sin(Qdesired() + Qdesired(2))]; = [L*cos(Q()) + L2*cos(Q() + Q(2));... L*sin(Q()) + L2*sin(Q() + Q(2))];

11 9/23/9 :44 PM C:\Users\Matthew Spenko\Documents\Classes\MMAE...\Homework_4_2.m 2 of 2 tilda = desired - ; %set the joint torques Kp = 5; Kd = 5; Torque = J'*(Kp*tilda - Kd*dot); qddot = inv(h)*(torque-c*[qdot; q2dot]); %compute the acceleration with gravity answer = [qdot; qddot(); q2dot; qddot(2)];