Commit 2c7faa5c authored by Amira Abdel-Rahman's avatar Amira Abdel-Rahman
Browse files

first commit

parent 5590f6b5
function simulate_leg(z0,tf)
%% Definte fixed paramters
m1 =.0393 + .2; m2 =.0368;
m3 = .00783; m4 = .0155;
I1 = 25.1 * 10^-6; I2 = 53.5 * 10^-6;
I3 = 9.25 * 10^-6; I4 = 22.176 * 10^-6;
l_OA=.011; l_OB=.042;
l_AC=.096; l_DE=.091;
l_O_m1=0.032; l_B_m2=0.0344;
l_A_m3=0.0622; l_C_m4=0.0610;
N = 18.75;
Ir = 0.0035/N^2;
g = 9.81;
restitution_coeff = 0.1;
% restitution_coeff = 0.;
friction_coeff = 0.3; %0.4-1.2
ground_height = -0.175;
%% Parameter vector
p = [m1 m2 m3 m4 I1 I2 I3 I4 Ir N l_O_m1 l_B_m2 l_A_m3 l_C_m4 l_OA l_OB l_AC l_DE g]';
%% Simulation Parameters Set 2 -- Operational Space Control
p_traj.omega = 30;
p_traj.omega = 3;
p_traj.x_0 = 0;
p_traj.y_0 = -.125-0.05;
p_traj.r = 0.025*1.5;
%% Perform Dynamic simulation
dt = 0.001;
num_step = floor(tf/dt);
tspan = linspace(0, tf, num_step);
z_out = zeros(8,num_step);
z_out(:,1) = z0;
for i=1:num_step-1
dz = dynamics(tspan(i), z_out(:,i), p, p_traj);
% Velocity update with dynamics
z_out(:,i+1) = z_out(:,i) + dz*dt;
% z_out(5:6,i+1) = joint_limit_constraint(z_out(:,i+1),p);
z_out(5:8,i+1) = discrete_impact_contact(z_out(:,i+1),p, restitution_coeff, friction_coeff, ground_height);
% Position update
z_out(1:4,i+1) = z_out(1:4,i) + z_out(5:8,i+1)*dt;
end
%% Compute foot position over time
rE = zeros(2,length(tspan));
vE = zeros(2,length(tspan));
for i = 1:length(tspan)
rE(:,i) = position_foot(z_out(:,i),p);
vE(:,i) = velocity_foot(z_out(:,i),p);
end
%% Animate Solution
figure(6); clf;
hold on
% Target traj
TH = 0:.1:2*pi;
plot( p_traj.x_0 + p_traj.r * cos(TH), ...
p_traj.y_0 + p_traj.r * sin(TH),'k--');
% Ground Q2.3
plot([-.2 .2],[ground_height ground_height],'k');
animateSol(tspan, z_out,p);
end
%Control Law
function tau = control_law(t, z, p, p_traj)
% Controller gains, Update as necessary for Problem 1
K_x = 150.; % Spring stiffness X
K_y = 150.; % Spring stiffness Y
D_x = 10.; % Damping X
D_y = 10.; % Damping Y
% Desired position of foot is a circle
omega_swing = p_traj.omega;
rEd = [p_traj.x_0 p_traj.y_0 0]' + ...
p_traj.r*[cos(omega_swing*t) sin(omega_swing*t) 0]';
% Compute desired velocity of foot
vEd = p_traj.r*[-sin(omega_swing*t)*omega_swing ...
cos(omega_swing*t)*omega_swing 0]';
% Desired acceleration
aEd = p_traj.r*[-cos(omega_swing*t)*omega_swing^2 ...
-sin(omega_swing*t)*omega_swing^2 0]';
% Actual position and velocity
rE = position_foot(z,p);
vE = velocity_foot(z,p);
% Jacobian matrix \partial r_E / \partial q
J = jacobian_foot(z,p);
dJ = jacobian_dot_foot(z,p);
J=J(1:2,1:2)
dJ=dJ(1:2,1:2)
dq = z(5:6);
% Compute virtual foce for Question 1.4 and 1.5
f = [K_x * (rEd(1) - rE(1) ) + D_x * (vEd(1) - vE(1) ) ;
K_y * (rEd(2) - rE(2) ) + D_y * (vEd(2) - vE(2) ) ];
%% Task-space compensation and feed forward for Question 1.8
% Get joint space components of equations of motion
Mass_Joint_Sp = A_leg(z,p);
Grav_Joint_Sp = Grav_leg(z,p);
Corr_Joint_Sp = Corr_leg(z,p);
Mass_Joint_Sp=Mass_Joint_Sp(1:2,1:2);
Grav_Joint_Sp=Grav_Joint_Sp(1:2,1:2);
Corr_Joint_Sp=Corr_Joint_Sp(1:2,1:2);
Mass_Joint_Sp_inv = inv(Mass_Joint_Sp);
% Task-space mass matrix (Equaiton 51 in Khatib's paper)
Lambda = inv(J * Mass_Joint_Sp_inv * J');
% Coriolis force in task-space (Equation 51)
mu = Lambda*J*Mass_Joint_Sp_inv* Corr_Joint_Sp - Lambda * dJ * dq;
% Gravity force in task-space (Equation 51)
rho = Lambda*J*Mass_Joint_Sp_inv * Grav_Joint_Sp;
% Add task-space acceleration force feedforward, coriolis, and gravity compensation
f(1:2) = Lambda*(aEd(1:2) + f(1:2)) + mu + rho; % OSC
% f(1:2) = Lambda*(aEd(1:2) + f(1:2)) + rho; % OSC w/o mu (coriolis)
% f(1:2) = Lambda*(aEd(1:2) + f(1:2)) + mu; % OSC w/o rho (gravity)
% Map to joint torques
tau = J' * f;
end
function tau = control_lawOldAmira(t, z, p, p_traj)
% Controller gains, Update as necessary for Problem 1
K_x = 150.; % Spring stiffness X
K_y = 150.; % Spring stiffness Y
D_x = 10.; % Damping X
D_y = 10.; % Damping Y
% Desired position of foot is a circle
omega_swing = p_traj.omega;
rEd = [p_traj.x_0 p_traj.y_0 0]' + ...
p_traj.r*[cos(omega_swing*t) sin(omega_swing*t) 0]';
% Compute desired velocity of foot
vEd = p_traj.r*[-sin(omega_swing*t)*omega_swing ...
cos(omega_swing*t)*omega_swing 0]';
% Desired acceleration
aEd = p_traj.r*[-cos(omega_swing*t)*omega_swing^2 ...
-sin(omega_swing*t)*omega_swing^2 0]';
% Actual position and velocity
rE = position_foot(z,p);
vE = velocity_foot(z,p);
% Compute virtual foce for Question 1.4 and 1.5
f = [K_x * (rEd(1) - rE(1) ) + D_x * ( - vE(1) ) ;
K_y * (rEd(2) - rE(2) ) + D_y * ( - vE(2) ) ];
% 1.1
M=A_leg(z,p);
% M=M(1:2,1:2);
invM=inv(M);
J = jacobian_foot(z,p);
dJ = jacobian_dot_foot(z,p);
% J=J(1:2,1:2);
% dJ=dJ(1:2,1:2);
q = z(1:4);
dq = z(5:8);
% q = z(1:2);
% dq = z(5:6);
Lambda=inv(J*invM*J');
Corr=Corr_leg(z,p);
Grav=Grav_leg(z,p);
% Corr=Corr(1:2,:);
% Grav=Grav(1:2,:);
nu=Lambda*J*invM*Corr -Lambda*dJ*dq ;
ro=Lambda*J*invM*Grav;
%% Task-space compensation and feed forward for Question 1.8
% Map to joint torques
%tau = J' * f;
K=[K_x;K_y];
D=[D_x;D_y];
% tau = J' *((Lambda *(aEd(1:2)+K.*(rEd(1:2)-rE)+D.*(vEd(1:2)-vE))+nu));
% tau = J' *((Lambda *(aEd(1:2)+K.*(rEd(1:2)-rE)+D.*(vEd(1:2)-vE))+ro));
tau = J' *((Lambda *(aEd(1:2)+K.*(rEd(1:2)-rE)+D.*(vEd(1:2)-vE))+nu+ro));
end
function dz = dynamics(t,z,p,p_traj)
% Get mass matrix
A = A_leg(z,p);
% Compute Controls
tau = control_law(t,z,p,p_traj);
% Get b = Q - V(q,qd) - G(q)
b = b_leg(z,tau,p);
% Solve for qdd.
qdd = A\(b);
dz = 0*z;
% Form dz
dz(1:4) = z(5:8);
dz(5:8) = qdd;
end
%% Discrete Contact
function qdot = discrete_impact_contact(z,p, rest_coeff, fric_coeff, yC)
qdot = z(3:4);
rE = position_foot(z, p);
vE = velocity_foot(z, p);
if(rE(2)-yC<0 && vE(2) < 0)
J = jacobian_foot(z,p);
A = A_leg(z,p);
Ainv = inv(A);
J_z = J(2,:);
lambda_z = 1/(J_z * Ainv * J_z.');
F_z = lambda_z*(-rest_coeff*vE(2) - J_z*qdot);
qdot = qdot + Ainv*J_z.'*F_z;
% horizontal
J_x = J(1,:);
lambda_x = 1/(J_x * Ainv * J_x.');
F_x = lambda_x * (0 - J_x * qdot);
if( abs(F_x) > fric_coeff*F_z)
F_x = sign(F_x)*F_z*fric_coeff;
end
qdot = qdot + Ainv*J_x.'*F_x;
z_test = z;
z_test(5:6) = qdot;
vE = velocity_foot(z_test, p);
end
end
function qdot = discrete_impact_contactOldAmira(z,p, rest_coeff, fric_coeff, yC)
rE = position_foot(z,p);
vE = velocity_foot(z,p);
J = jacobian_foot(z,p);
%J=J(1:2,1:2);
Jcx=J(1,:);
Jcy=J(2,:);
M=A_leg(z,p);
% M=M(1:2,1:2);
invM=inv(M);
q=z(1:4);
dq=z(5:8);
%q = z(1:2);
%dq = z(5:6);
Lambda=inv(J*invM*J');
Lambda_cx=inv(Jcx*invM*Jcx');
Lambda_cy=inv(Jcy*invM*Jcy');
y=rE(2);
dy=vE(2);
Cy=y-yC;
dCy=dy;
qdot=dq;
if(Cy<0 && dCy<0)
Fcy=Lambda_cy*(-rest_coeff*dCy-Jcy*dq);
qdot=qdot+invM*Jcy'*Fcy;
Fcx=Lambda_cx*(0-Jcx*dq);
if(Fcx<-fric_coeff*Fcy)
Fcx=-fric_coeff*Fcy;
end
if(Fcx>fric_coeff*Fcy)
Fcx=fric_coeff*Fcy;
end
qdot=qdot+invM*Jcx'*Fcx;
end
end
%% joint Constraint
function qdot = joint_limit_constraint(z,p)
% qdot=z(5:6);
% if z(1)<-50*3.14/180 || z(1)>150*3.14/180 || z(2)<5*3.14/180 || z(2)>150*3.14/180
% qdot=[0;0];
% end
%%
q1_min = -50 * pi/ 180;
C = z(1) - q1_min; % C gives distance away from constraint
dC= z(5);
qdot = z(5:6);
J = [1 0];
A = A_leg(z,p);
if (C < 0 && dC <0)% if constraint is violated
lambda = A(2,2);
F_c = lambda * (0 - dC);
qdot = qdot + inv(A)*J.'*F_c;
end
end
function animateSol(tspan, x,p)
% Prepare plot handles
hold on
h_OB = plot([0],[0],'LineWidth',2);
h_AC = plot([0],[0],'LineWidth',2);
h_BD = plot([0],[0],'LineWidth',2);
h_CE = plot([0],[0],'LineWidth',2);
xlabel('x'); ylabel('y');
h_title = title('t=0.0s');
axis equal
axis([-.2 .2 -.3 .1]);
%Step through and update animation
for i = 1:length(tspan)
% skip frame.
if mod(i,10)
continue;
end
t = tspan(i);
z = x(:,i);
keypoints = keypoints_leg(z,p);
r0 = keypoints(:,6);
rA = keypoints(:,1); % Vector to base of cart
rB = keypoints(:,2);
rC = keypoints(:,3); % Vector to tip of pendulum
rD = keypoints(:,4);
rE = keypoints(:,5);
set(h_title,'String', sprintf('t=%.2f',t) ); % update title
set(h_OB,'XData',[r0(1) rB(1)]);
set(h_OB,'YData',[r0(2) rB(2)]);
set(h_AC,'XData',[rA(1) rC(1)]);
set(h_AC,'YData',[rA(2) rC(2)]);
set(h_BD,'XData',[rB(1) rD(1)]);
set(h_BD,'YData',[rB(2) rD(2)]);
set(h_CE,'XData',[rC(1) rE(1)]);
set(h_CE,'YData',[rC(2) rE(2)]);
pause(.01)
end
end
\ No newline at end of file
......@@ -19,9 +19,10 @@ function simulate_leg(z0,tf)
Ir = 0.0035/N^2;
g = 9.81;
restitution_coeff = 0.1;
% restitution_coeff = 0.;
% restitution_coeff = 0.1;
restitution_coeff = 0.;
friction_coeff = 0.3; %0.4-1.2
friction_coeff=10
ground_height = -0.175;
%% Parameter vector
......@@ -32,6 +33,7 @@ function simulate_leg(z0,tf)
p_traj.omega = 3;
p_traj.x_0 = 0;
p_traj.y_0 = -.125-0.05;
% p_traj.y_0 = -.125;
p_traj.r = 0.025*1.5;
%% Perform Dynamic simulation
......@@ -50,8 +52,8 @@ function simulate_leg(z0,tf)
% Velocity update with dynamics
z_out(:,i+1) = z_out(:,i) + dz*dt;
z_out(5:8,i+1) = discrete_impact_contact(z_out(:,i+1),p, restitution_coeff, friction_coeff, ground_height);
z_out(5:6,i+1) = joint_limit_constraint(z_out(:,i+1),p);
z_out(5:8,i+1) = discrete_impact_contact(z_out(:,i+1),p, restitution_coeff, friction_coeff, ground_height);
% Position update
z_out(1:4,i+1) = z_out(1:4,i) + z_out(5:8,i+1)*dt;
......@@ -84,49 +86,74 @@ function simulate_leg(z0,tf)
animateSol(tspan, z_out,p);
end
function tau = control_law1(t,z,p,p_traj)
%Control Law
function tau = control_law(t, z, p, p_traj)
% Controller gains, Update as necessary for Problem 1
K_x = 10; % Spring stiffness X
K_y = 10; % Spring stiffness Y
D_x = 0; % Damping X
D_y = 0; % Damping Y
K_x = 100; % Spring stiffness X
K_y = 100; % Spring stiffness Y
D_x = 4; % Damping X
D_y = 4; % Damping Y
K_x = 150.; % Spring stiffness X
K_y = 150.; % Spring stiffness Y
D_x = 10.; % Damping X
D_y = 10.; % Damping Y
rEd = [.025 -.125 0]'; % Desired position of foot
w=3;
% w=30;
rEd = [0.025*cos(w*t) -.125+0.025*sin(w*t)]'; % Desired position of foot
%% STEPS TO COMPLETE PROBLEM 1.3
% a. Compute r_E
% Desired position of foot is a circle
omega_swing = p_traj.omega;
rEd = [p_traj.x_0 p_traj.y_0 0]' + ...
p_traj.r*[cos(omega_swing*t) sin(omega_swing*t) 0]';
% Compute desired velocity of foot
vEd = p_traj.r*[-sin(omega_swing*t)*omega_swing ...
cos(omega_swing*t)*omega_swing 0]';
% Desired acceleration
aEd = p_traj.r*[-cos(omega_swing*t)*omega_swing^2 ...
-sin(omega_swing*t)*omega_swing^2 0]';
% Actual position and velocity
rE = position_foot(z,p);
% b. Compute J, the jacobian of r_E
J=jacobian_foot(z,p);
vE = velocity_foot(z,p);
% Jacobian matrix \partial r_E / \partial q
J = jacobian_foot(z,p);
dJ = jacobian_dot_foot(z,p);
J=J(1:2,1:2);
% c. Use these results to compute \tau as specified in the write-up
e=rE-rEd;
e_x=e(1);
e_y=e(2);
v=velocity_foot(z,p);
dJ=dJ(1:2,1:2);
dq = z(5:6);
% Compute virtual foce for Question 1.4 and 1.5
f = [K_x * (rEd(1) - rE(1) ) + D_x * (vEd(1) - vE(1) ) ;
K_y * (rEd(2) - rE(2) ) + D_y * (vEd(2) - vE(2) ) ];
%% Task-space compensation and feed forward for Question 1.8
% Get joint space components of equations of motion
Mass_Joint_Sp = A_leg(z,p);
Grav_Joint_Sp = Grav_leg(z,p);
Corr_Joint_Sp = Corr_leg(z,p);
Mass_Joint_Sp=Mass_Joint_Sp(1:2,1:2);
Grav_Joint_Sp=Grav_Joint_Sp(1:2);
Corr_Joint_Sp=Corr_Joint_Sp(1:2);
Mass_Joint_Sp_inv = inv(Mass_Joint_Sp);
% Task-space mass matrix (Equaiton 51 in Khatib's paper)
Lambda = inv(J * Mass_Joint_Sp_inv * J');
% Coriolis force in task-space (Equation 51)
mu = Lambda*J*Mass_Joint_Sp_inv* Corr_Joint_Sp - Lambda * dJ * dq;
% F=-[K_x*J(1)*e_x;K_y*J(2)*e_y;0];
% Gravity force in task-space (Equation 51)
rho = Lambda*J*Mass_Joint_Sp_inv * Grav_Joint_Sp;
%F=-[0.5*K_x*e_x^2+D_x*v(1);0.5*K_y*e_y^2+D_y*v(2);0];
% Add task-space acceleration force feedforward, coriolis, and gravity compensation
f(1:2) = Lambda*(aEd(1:2) + f(1:2)) + mu + rho; % OSC
% f(1:2) = Lambda*(aEd(1:2) + f(1:2)) + rho; % OSC w/o mu (coriolis)
% f(1:2) = Lambda*(aEd(1:2) + f(1:2)) + mu; % OSC w/o rho (gravity)
F=-[K_x*e_x+D_x*v(1);K_y*e_y+D_y*v(2)];
% Hint: Some of the functions generated by derive_leg will help with
% steps a and b.
tau =J'*(F);
% Map to joint torques
tau = J' * f;
end
function tau = control_law(t, z, p, p_traj)
function tau = control_lawOldAmira(t, z, p, p_traj)
% Controller gains, Update as necessary for Problem 1
K_x = 150.; % Spring stiffness X
K_y = 150.; % Spring stiffness Y
......@@ -213,6 +240,37 @@ end
%% Discrete Contact
function qdot = discrete_impact_contact(z,p, rest_coeff, fric_coeff, yC)
qdot = z(5:8);
rE = position_foot(z, p);
vE = velocity_foot(z, p);
if(rE(2)-yC<0 && vE(2) < 0)
J = jacobian_foot(z,p);
A = A_leg(z,p);
Ainv = inv(A);
J_z = J(2,:);
lambda_z = 1/(J_z * Ainv * J_z.');
F_z = lambda_z*(-rest_coeff*vE(2) - J_z*qdot);
qdot = qdot + Ainv*J_z.'*F_z;
% horizontal
J_x = J(1,:);
lambda_x = 1/(J_x * Ainv * J_x.');
F_x = lambda_x * (0 - J_x * qdot);
if( abs(F_x) > fric_coeff*F_z)
F_x = sign(F_x)*F_z*fric_coeff;
end
qdot = qdot + Ainv*J_x.'*F_x;
z_test = z;
z_test(5:8) = qdot;
vE = velocity_foot(z_test, p);
end
end
function qdot = discrete_impact_contactOldAmira(z,p, rest_coeff, fric_coeff, yC)
rE = position_foot(z,p);
vE = velocity_foot(z,p);
J = jacobian_foot(z,p);
......@@ -261,11 +319,30 @@ function qdot = discrete_impact_contact(z,p, rest_coeff, fric_coeff, yC)
end
%% joint Constraint
function qdot = joint_limit_constraint(z,p)
qdot=z(5:6);
if z(1)<-50*3.14/180 || z(1)>150*3.14/180 || z(2)<5*3.14/180 || z(2)>150*3.14/180
qdot=[0;0];
end
%%
% q1_min = -50 * pi/ 180;
% C = z(1) - q1_min; % C gives distance away from constraint
% dC= z(5);
%
%
% qdot = z(5:6);
% J = [1 0];
% A = A_leg(z,p);
%
%
% if (C < 0 && dC <0)% if constraint is violated
% lambda = A(2,2);
% F_c = lambda * (0 - dC);
% qdot = qdot + inv(A)*J.'*F_c;
% end
end
......
z0 = [-pi/4; pi/2; 0; 0; 0; 0; 0; 0];
z0 = [-30*pi/180; 90*pi/180; 0; 0; 0; 0; 0; 0];
tf = 1;
simulate_leg(z0,tf)
\ No newline at end of file
No preview for this file type
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment