Commit 0e819fbc authored by Amira Abdel-Rahman's avatar Amira Abdel-Rahman
Browse files

change control law, and first path for optimization

parent 2c7faa5c
function u = BezierCurve(P, t)
n = size(P,2);
for i=1:n
for j=1:n-i
P(:,j) = (1-t)*P(:,j) + t*P(:,j+1);
end
end
u = P(:,1);
% n = length(ctrl_pt);
% u = 0;
% for i = 1:n
% u = 1; % compute return value. Write your code instead of 1.
% end
end
\ No newline at end of file
function p=get_parameters()
%% 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;
%% 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]';
end
\ No newline at end of file
function simulate_leg(z0,tf)
function [t_out, z_out, u_out] =simulate_leg(z0,tf,ctrl1,ctrl2)
%% 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;
......@@ -19,10 +16,11 @@ function simulate_leg(z0,tf)
Ir = 0.0035/N^2;
g = 9.81;
restitution_coeff = 0.1;
% restitution_coeff = 0.;
friction_coeff = 0.3; %0.4-1.2
restitution_coeff = 0.;
%friction_coeff = 0.3; %0.4-1.2
friction_coeff=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]';
......@@ -32,29 +30,38 @@ 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
dt = 0.001;
num_step = floor(tf/dt);
tspan = linspace(0, tf, num_step);
t_out=tspan;
u_out = zeros(2,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);
[dz,u] = dynamics(tspan(i), z_out(:,i), p, ctrl1,ctrl2);
% 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: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;
u_out(:,i+1)=u;
end
......@@ -74,7 +81,7 @@ function simulate_leg(z0,tf)
% Target traj
TH = 0:.1:2*pi;
%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--');
......@@ -84,14 +91,23 @@ function simulate_leg(z0,tf)
animateSol(tspan, z_out,p);
end
%Control Law
function tau = control_law(t, z, p, p_traj)
%Torque Control Law
function tau = control_law_torque(t, z, p,ctrl1,ctrl2 )
tau1 = BezierCurve(ctrl1.T, t/ctrl1.tf);
tau2 = BezierCurve(ctrl2.T, t/ctrl2.tf);
tau =[tau1;tau2];
end
%Impedance Control Law
function tau = control_law_Impedance(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]' + ...
......@@ -110,8 +126,8 @@ function tau = control_law(t, z, p, p_traj)
% 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)
J=J(1:2,1:2);
dJ=dJ(1:2,1:2);
dq = z(5:6);
......@@ -124,10 +140,10 @@ function tau = control_law(t, z, p, p_traj)
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);
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)
......@@ -148,78 +164,14 @@ function tau = control_law(t, z, p, p_traj)
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)
function [dz,tau] = dynamics(t,z,p,ctrl1,ctrl2)
% Get mass matrix
A = A_leg(z,p);
% Compute Controls
tau = control_law(t,z,p,p_traj);
tau = control_law_torque(t,z,p,ctrl1,ctrl2);
%tau = control_law(t,z,p,p_traj);
% Get b = Q - V(q,qd) - G(q)
b = b_leg(z,tau,p);
......@@ -233,11 +185,10 @@ function dz = dynamics(t,z,p,p_traj)
dz(5:8) = qdd;
end
%% Discrete Contact
function qdot = discrete_impact_contact(z,p, rest_coeff, fric_coeff, yC)
qdot = z(3:4);
qdot = z(5:8);
rE = position_foot(z, p);
vE = velocity_foot(z, p);
......@@ -260,85 +211,37 @@ function qdot = discrete_impact_contact(z,p, rest_coeff, fric_coeff, yC)
end
qdot = qdot + Ainv*J_x.'*F_x;
z_test = z;
z_test(5:6) = qdot;
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);
%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
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
% 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
......@@ -391,4 +294,6 @@ function animateSol(tspan, x,p)
pause(.01)
end
end
\ No newline at end of file
end
function simulate_leg(z0,tf)
%% Definte fixed paramters
m1 =.0393 + .2; m2 =.0368;
m3 = .00783; m4 = .0155;
function [t_out, z_out, u_out] =simulate_leg(z0,p,ground,tf,ctrl1,ctrl2,showPlot)
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
friction_coeff=10
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
%% to remove
p_traj.omega = 30;
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
dt = 0.001;
num_step = floor(tf/dt);
tspan = linspace(0, tf, num_step);
t_out=tspan;
u_out = zeros(2,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);
[dz,u] = dynamics(tspan(i), z_out(:,i), p, ctrl1,ctrl2);
% 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);
z_out(5:8,i+1) = discrete_impact_contact(z_out(:,i+1),p, ground);
% Position update
z_out(1:4,i+1) = z_out(1:4,i) + z_out(5:8,i+1)*dt;
u_out(:,i+1)=u;
end
......@@ -70,24 +48,34 @@ function simulate_leg(z0,tf)
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');
%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--');
animateSol(tspan, z_out,p);
if(showPlot)
%% Animate Solution
figure(6); clf;
hold on
plot([-.2 .2],[ground.ground_height ground.ground_height],'k');
animateSol(tspan, z_out,p);
end
end
%Control Law
function tau = control_law(t, z, p, p_traj)
%Torque Control Law
function tau = control_law_torque(t, z, p,ctrl1,ctrl2 )
tau1 = BezierCurve(ctrl1.T, t/ctrl1.tf);
tau2 = BezierCurve(ctrl2.T, t/ctrl2.tf);
tau =[tau1;tau2];
end
%Impedance Control Law
function tau = control_law_Impedance(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
......@@ -152,78 +140,14 @@ function tau = control_law(t, z, p, p_traj)
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)
function [dz,tau] = dynamics(t,z,p,ctrl1,ctrl2)
% Get mass matrix
A = A_leg(z,p);
% Compute Controls
tau = control_law(t,z,p,p_traj);
tau = control_law_torque(t,z,p,ctrl1,ctrl2);
%tau = control_law(t,z,p,p_traj);
% Get b = Q - V(q,qd) - G(q)
b = b_leg(z,tau,p);
......@@ -237,10 +161,12 @@ function dz = dynamics(t,z,p,p_traj)
dz(5:8) = qdd;
end
%% Discrete Contact
function qdot = discrete_impact_contact(z,p, rest_coeff, fric_coeff, yC)
function qdot = discrete_impact_contact(z,p,ground)
rest_coeff=ground.restitution_coeff;
fric_coeff=ground.friction_coeff;
yC=ground.ground_height;
qdot = z(5:8);
rE = position_foot(z, p);
vE = velocity_foot(z, p);
......@@ -270,59 +196,11 @@ function qdot = discrete_impact_contact(z,p, rest_coeff, fric_coeff, yC)
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)
</