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

casadi

parent 49ecc665
casadi
\ No newline at end of file
casadi
casadi_mac
\ No newline at end of file
......@@ -27,7 +27,7 @@ function p=get_parameters()
N = 18.75;
Ir = 0.0035/N^2;
g = 9.81/4;
g = 9.81/2;
%% 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]';
p = [m0 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]'; % parameters
......
function [t_out, z_out, u_out] =simulate_leg(z0,p,ground,tf,ctrl1,ctrl2)
%% to remove
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);
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,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, 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
%% 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
% 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--');
end
%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]' + ...
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);
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;
% 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 [dz,tau] = dynamics(t,z,p,ctrl1,ctrl2)
% Get mass matrix
A = A_leg(z,p);
% Compute Controls
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);
% 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,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);
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
%% joint Constraint
function z1 = joint_limit_constraint(z,p)
z1=z;
qdot=z(5:6);
% if z(2)<(90-45)*pi/180 || z(2)>(90+55)*pi/180
if z(2)<(90-45)*pi/180 || z(2)>(90+55)*pi/180
qdot=[0;0];
end
if z(2)<(90-45)*pi/180
end
function qdot = joint_limit_constraint1(z,p)
%%
q2_min = (90-45) * pi/ 180;
C = z(2) - q2_min; % C gives distance away from constraint
dC= z(6);
qdot = z(5:6);
J = [0 1];
A = A_leg(z,p);
A=A(1:2,1:2);
if (C < 0 && dC <0)% if constraint is violated
lambda = A(1,1);
F_c = lambda * (0 - dC);
qdot = qdot + inv(A)*J.'*F_c;
end
%%
q2_max = (90+55) * pi/ 180;
C = q2_max-z(2) ; % C gives distance away from constraint
dC= z(6);
if (C < 0 && dC >0)% if constraint is violated
lambda = A(1,1);
F_c = lambda * (0 + dC);
qdot = qdot + inv(A)*J.'*F_c;
end
qdot =qdot(1:2);
end
......@@ -28,8 +28,8 @@ function [t_out, z_out, u_out] =simulate_leg(z0,p,ground,tf,ctrl1,ctrl2)
% Velocity update with dynamics
z_out(:,i+1) = z_out(:,i) + dz*dt;
z_out(5:8,i+1) = joint_limit_constraint1(z_out(:,i+1),p);
% z_out(:,i+1) = joint_limit_constraint(z_out(:,i+1),p);
% z_out(5:8,i+1) = joint_limit_constraint1(z_out(:,i+1),p);
z_out(:,i+1) = joint_limit_constraint(z_out(:,i+1),p);
z_out(5:8,i+1) = discrete_impact_contact(z_out(:,i+1),p, ground);
......
function [t_out, z_out, u_out] =simulate_leg_casadi(z0,p,ground,ctrl1,ctrl2,ctrl3)
num_step=ctrl1.n+ctrl2.n+ctrl3.n;
% Declare casadi symbolic variables
t_out = casadi.MX(zeros(1,num_step));
z_out = casadi.MX(zeros(8,num_step));
u_out = casadi.MX(zeros(2,num_step-1));
% Time discretization
t_out(1) = 0;
dt1 = ctrl1.tf/(ctrl1.n-1);
dt2 = ctrl1.tf/(ctrl2.n-1);
dt3 = ctrl1.tf/(ctrl3.n-1);
i=2;
for j = 1:ctrl1.n-1
t_out(i) = t_out(i-1) + dt1;
i=i+1;
end
for j = 1:ctrl2.n
t_out(i) = t_out(i-1) + dt2;
i=i+1;
end
for j = 1:ctrl3.n
t_out(i) = t_out(i-1) + dt3;
i=i+1;
end
%% Perform Dynamic simulation
z_out(:,1) = z0;
for j=1:ctrl1.n-1
i=j;
t = t_out(i);
[dz,u] = dynamics(t, z_out(:,i), p, ctrl1);
% Velocity update with dynamics
z_out(:,i+1) = z_out(:,i) + dz*dt1;
z_out(5:8,i+1) = discrete_impact_contact_stance(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)*dt1 ;
u_out(:,i)=u;
end
for j=1:ctrl2.n
i=j+ctrl1.n;
t = t_out(i);
[dz,u] = dynamics(t, z_out(:,i), p, ctrl2);
% Velocity update with dynamics
z_out(:,i+1) = z_out(:,i) + dz*dt2;
z_out(5:8,i+1) = discrete_impact_contact_flight(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)*dt2 ;
u_out(:,i)=u;
end
for j=1:ctrl3.n
i=j+ctrl2.n+ctrl3.n;
t = t_out(i);
[dz,u] = dynamics(t, z_out(:,i), p, ctrl3);
% Velocity update with dynamics
z_out(:,i+1) = z_out(:,i) + dz*dt3;
z_out(5:8,i+1) = discrete_impact_contact_stance(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)*dt3 ;
u_out(:,i)=u;
end
end
%Torque Control Law
function tau = control_law_torque(t, z, p,ctrl1 )
tau1 = BezierCurve(ctrl.q1T, t/ctrl.tf);
tau2 = BezierCurve(ctrl.q2T, t/ctrl.tf);
tau =[tau1;tau2];
end
function [dz,tau] = dynamics(t,z,p,ctrl)
% Get mass matrix
A = A_leg(z,p);
% Compute Controls
tau = control_law_torque(t,z,p,ctrl);
%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;
dz = casadi.MX(zeros(8,1));
% Form dz
dz(1:4,1) = z(5:8);
dz(5:8,1) = qdd;
end
%% Discrete Contact stance
function qdot = discrete_impact_contact_stance(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);
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
%% Discrete Contact stance
function qdot = discrete_impact_contact_flight(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);
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
%% 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
if z(2)<(90-45)*3.14/180 || z(2)>(90+55)*3.14/180
qdot=[0;0];
end
%%
% q2_min = 50 * pi/ 180;
% C1 = z(2) - q2_min; % C gives distance away from constraint
% dC= z(6);
% q2_max = 150 * pi/ 180;
% C2 = q2_max-z(2); % C gives distance away from constraint
% qdot = z(5:6);
% J = [1 0];
% A = A_leg(z,p);
% A=A(1:2,1:2);
% % size(A)
% if ( (C1 < 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 [t_out, z_out, u_out] =simulate_leg_casadi(z0,p,ground,tf,ctrl1,ctrl2,N)
function [t_out, z_out, u_out] =simulate_leg_casadi(z0,p,ground,ctrl1,ctrl2,ctrl3)
num_step=ctrl1.n+ctrl2.n+ctrl3.n;
% Declare casadi symbolic variables
t_out = casadi.MX(zeros(1,N.ctrl));
z_out = casadi.MX(zeros(8,N.ctrl));
u_out = casadi.MX(zeros(2,N.ctrl-1));
t_out = casadi.MX(zeros(1,num_step));
z_out = casadi.MX(zeros(8,num_step));
u_out = casadi.MX(zeros(2,num_step-1));
%% Perform Dynamic simulation
dt = tf/(N.ctrl-1);
% Time discretization
t_out(1) = 0;
dt1 = ctrl1.tf/(ctrl1.n-1);
dt2 = ctrl1.tf/(ctrl2.n-1);
dt3 = ctrl1.tf/(ctrl3.n-1);
i=2;
for j = 1:ctrl1.n-1
t_out(i) = t_out(i-1) + dt1;
i=i+1;
end
for j = 1:ctrl2.n
t_out(i) = t_out(i-1) + dt2;
i=i+1;
end
for j = 1:ctrl3.n
t_out(i) = t_out(i-1) + dt3;
i=i+1;
end
num_step = N.ctrl;
%% Perform Dynamic simulation
z_out(:,1) = z0;
for i=1:num_step-1
for j=1:ctrl1.n
i=j;
t = t_out(i);
[dz,u] = dynamics(t, z_out(:,i), p, ctrl1);
% Velocity update with dynamics
z_out(:,i+1) = z_out(:,i) + dz*dt1;
z_out(5:8,i+1) = discrete_impact_contact_stance(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)*dt1 ;
u_out(:,i)=u;
end
for j=1:ctrl2.n
i=j+ctrl1.n;
t = t_out(i);
[dz,u] = dynamics(t, z_out(:,i), p, ctrl2);
% Velocity update with dynamics
z_out(:,i+1) = z_out(:,i) + dz*dt2;
% Position update
z_out(1:4,i+1) = z_out(1:4,i) + z_out(5:8,i+1)*dt2 ;
u_out(:,i)=u;
end
for j=1:ctrl3.n-1
i=j+ctrl2.n+ctrl3.n;
t = t_out(i);
[dz,u] = dynamics(t, z_out(:,i), p, ctrl1,ctrl2);
% dz = dynamics(tspan(i), z_out(:,i), p, p_traj);
[dz,u] = dynamics(t, z_out(:,i), p, ctrl3);
% Velocity update with dynamics
z_out(:,i+1) = z_out(:,i) + dz*dt;
z_out(:,i+1) = z_out(:,i) + dz*dt3;
% 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, ground);
z_out(5:8,i+1) = discrete_impact_contact_stance(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 ;
z_out(1:4,i+1) = z_out(1:4,i) + z_out(5:8,i+1)*dt3 ;
u_out(:,i)=u;
end
end
%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);
function tau = control_law_torque(t, z, p,ctrl)
tau1 = BezierCurve(ctrl.q1T, t/ctrl.tf);
tau2 = BezierCurve(ctrl.q2T, t/ctrl.tf);
tau =[tau1;tau2];
end
function [dz,tau] = dynamics(t,z,p,ctrl1,ctrl2)
function [dz,tau] = dynamics(t,z,p,ctrl)
% Get mass matrix
A = A_leg(z,p);
% Compute Controls
tau = control_law_torque(t,z,p,ctrl1,ctrl2);
%tau = control_law(t,z,p,p_traj);
tau = control_law_torque(t,z,p,ctrl);