Initial commit.

Author: sea-bass

ControlDesign/InverseKinematics/animateFootGait.m

@@ -0,0 +1,108 @@
+% Visualize Walking Gait and Inverse Kinematics
+% Copyright 2019 The MathWorks, Inc.
+
+close all
+robotParametersInvKin;
+
+%% Plot the foot trajectory
+gaitPeriod = 1;
+stepLength = 0.1;
+stepHeight = 0.025;
+numPoints  = 150; 
+
+tVec = linspace(0,gaitPeriod,numPoints);
+foot_height_offset = sqrt( (lower_leg_length+upper_leg_length)^2 ...
+                         - ((stepLength/2)*100)^2 ) - 1e-3;
+
+figure, hold on
+x = zeros(numPoints,1);
+y = zeros(numPoints,1); 
+for idx = 1:numPoints   
+   [x(idx),y(idx)] = evalFootGait(tVec(idx),stepLength,stepHeight,gaitPeriod);
+end
+
+plot(x,y,'.-');
+axis equal
+title('Foot Gait');
+xlabel('x [m]')
+ylabel('y [m]')
+
+%% Calculate joint angles
+theta_hip = zeros(numPoints,1);
+theta_knee = zeros(numPoints,1);
+theta_ankle = zeros(numPoints,1);
+
+for idx = 1:numPoints
+    
+    pitch = 0; % Assume zero body pitch
+    
+    % Calculate inverse kinematics
+    theta = legInvKin(upper_leg_length/100, lower_leg_length/100 , ... 
+                      x(idx), y(idx) - (foot_height_offset/100));
+
+    % Address multiple solutions by preventing knee bending backwards
+    if size(theta,1) == 2
+       if theta(1,2) > 0
+          t1 = theta(2,1);
+          t2 = theta(2,2);
+       else
+          t1 = theta(1,1);
+          t2 = theta(1,2);
+       end
+    else
+        t1 = theta(1);
+        t2 = theta(2);
+    end
+    
+    % Pack the results. Ensure the ankle angle is set so the foot always
+    % lands flat on the ground
+    theta_hip(idx) = t1;
+    theta_knee(idx) = t2;
+    theta_ankle(idx) = -(t1+t2);
+    
+end
+
+% Display joint angles
+figure
+subplot(311)
+plot(tVec,rad2deg(theta_hip))
+title('Hip Angle [deg]');
+subplot(312)
+plot(tVec,rad2deg(theta_knee))
+title('Knee Angle [deg]');
+subplot(313)
+plot(tVec,rad2deg(theta_ankle))
+title('Ankle Angle [deg]');
+                     
+%% Animate the walking gait
+figure(3), clf, hold on
+
+% Initialize plot
+plot(x,y-foot_height_offset/100,'k:','LineWidth',1);
+h1 = plot([0 0],[0 0],'r-','LineWidth',4);
+h2 = plot([0 0],[0 0],'b-','LineWidth',4);
+
+% Calculate knee and ankle (x,y) positions
+xKnee =  sin(theta_hip)*upper_leg_length/100;
+yKnee = -cos(theta_hip)*upper_leg_length/100;
+xAnkle = xKnee + sin(theta_hip+theta_knee)*lower_leg_length/100;
+yAnkle = yKnee - cos(theta_hip+theta_knee)*lower_leg_length/100;
+
+% Define axis limits
+xMin = min([xKnee;xAnkle]) -0.025; 
+xMax = max([xKnee;xAnkle]) +0.025;
+yMin = min([yKnee;yAnkle]) -0.025; 
+yMax = max([0;yKnee;yAnkle]) +0.025;
+
+% Animate the walking gait
+numAnimations = 5;
+for anim = 1:numAnimations
+    for idx = 1:numPoints
+        set(h1,'xdata',[0 xKnee(idx)],'ydata',[0 yKnee(idx)]);
+        set(h2,'xdata',[xKnee(idx) xAnkle(idx)],'ydata',[yKnee(idx) yAnkle(idx)]);
+        xlim([xMin xMax]), ylim([yMin yMax]);
+        title('Walking Gait Animation');
+        axis equal
+        drawnow
+    end
+end

ControlDesign/InverseKinematics/calculateInvKin.mlx

@@ -0,0 +1,23 @@
+function [x,y] = evalFootGait(t,stepLength,stepHeight,gaitPeriod)
+% EVALFOOTGAIT Evaluates foot gait at a specified time
+%
+% Copyright 2019 The MathWorks, Inc.
+
+    % t = 0 : extended forward
+    % 0 >= t > T/2 : dragging back
+    % T/2 >= t > T : swinging forward
+    
+    tEff = mod(t,gaitPeriod);
+    
+    % Dragging back (y position is 0)
+    if tEff < gaitPeriod/2
+        x = stepLength * (gaitPeriod - 2*tEff)/gaitPeriod - stepLength/2;
+        y = 0;
+        
+    % Swinging forward (y follows curve)
+    else
+        x = stepLength * (2*tEff - gaitPeriod)/gaitPeriod - stepLength/2; 
+        y = stepHeight*(1 - (4*abs(tEff - 3*gaitPeriod/4)/gaitPeriod)^2);
+    end
+    
+end

ControlDesign/InverseKinematics/evalFootGait.m

@@ -0,0 +1,26 @@
+function out1 = legInvKin(L1,L2,x,y)
+%LEGINVKIN
+%    OUT1 = LEGINVKIN(L1,L2,X,Y)
+
+%    This function was generated by the Symbolic Math Toolbox version 8.2.
+%    02-Nov-2018 22:21:58
+
+t2 = L1.^2;
+t3 = L2.^2;
+t4 = x.^2;
+t5 = y.^2;
+t6 = L1.*L2.*2.0;
+t7 = -t2-t3+t4+t5+t6;
+t8 = t2+t3-t4-t5+t6;
+t9 = t7.*t8;
+t10 = sqrt(t9);
+t11 = 1.0./t7;
+t12 = t2-t3+t4+t5-L1.*y.*2.0;
+t13 = 1.0./t12;
+t14 = L1.*x.*2.0;
+t15 = t4.*t10.*t11;
+t16 = t5.*t10.*t11;
+t17 = L1.*L2.*t10.*t11.*2.0;
+t18 = t10.*t11;
+t19 = atan(t18);
+out1 = reshape([atan(t13.*(t14+t15+t16+t17-t2.*t10.*t11-t3.*t10.*t11)).*2.0,atan(t13.*(t14-t15-t16-t17+t2.*t10.*t11+t3.*t10.*t11)).*2.0,t19.*-2.0,t19.*2.0],[2,2]);

ControlDesign/InverseKinematics/legInvKin.m

@@ -0,0 +1,85 @@
+% Walking Robot Parameters -- Inverse Kinematics Variation
+% Copyright 2019 The MathWorks, Inc.
+
+%% General parameters
+density = 500;
+foot_density = 1000;
+world_damping = 1e-3;
+world_rot_damping = 0.25;
+if ~exist('actuatorType','var')
+    actuatorType = 1;
+end
+
+%% Inputs
+gaitPeriod = 0.75;
+stepLength = 0.075;
+stepHeight = 0.025;
+                     
+%% Contact/friction parameters
+contact_stiffness = 1000;
+contact_damping = 50;
+mu_k = 0.7;
+mu_s = 0.9;
+mu_vth = 0.1;
+height_plane = 0.025;
+plane_x = 25;
+plane_y = 3;
+contact_point_radius = 1e-4;
+
+%% Foot parameters
+foot_x = 8;
+foot_y = 6;
+foot_z = 1;
+foot_offset = [-1 0 0];
+
+%% Leg parameters
+leg_radius = 0.75;
+lower_leg_length = 10;
+upper_leg_length = 10;
+
+%% Torso parameters
+torso_y = 10;
+torso_x = 5;
+torso_z = 8;
+torso_offset_z = -2;
+torso_offset_x = -1;
+
+%% Derived parameters for initial conditions
+foot_height_offset = sqrt( (lower_leg_length+upper_leg_length)^2 ...
+                         - ((stepLength/2)*100)^2 ) - 1e-3;
+if ~isreal(foot_height_offset)
+    error('Step Length is too long given the robot leg lengths.');
+end
+
+init_height = foot_z + foot_height_offset + ...
+              torso_z/2 + torso_offset_z + height_plane/2;
+          
+init_ang = sin( (stepLength/2)*100 / sqrt(lower_leg_length+upper_leg_length)^2 );
+initAnglesR = init_ang * [1; 0; -1];
+initAnglesL = -initAnglesR;
+
+%% Joint parameters
+joint_damping = 1;
+joint_stiffness = 1;
+motion_time_constant = 0.001;
+
+%% Joint controller parameters
+hip_servo_kp = 60;
+hip_servo_ki = 10;
+hip_servo_kd = 20;
+knee_servo_kp = 60;
+knee_servo_ki = 5;
+knee_servo_kd = 10;
+ankle_servo_kp = 20;
+ankle_servo_ki = 4;
+ankle_servo_kd = 8;
+deriv_filter_coeff = 100;
+max_torque = 20;
+
+%% Electric motor parameters
+motor_resistance = 1;
+motor_constant = 0.02;
+motor_inertia = 0;
+motor_damping = 0;
+motor_inductance = 1.2e-6;
+gear_ratio = 50;