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

casadi first trial, velocity added

parent 020498e1
casadi
\ No newline at end of file
function u = BezierCurve(P, t)
% 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
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
function u = BezierCurve(ctrl_pt, t)
n = length(ctrl_pt);
u = 0;
for i = 1:n
u = u + factorial(n-1)/(factorial(i-1) * factorial(n-i)) * t^(i-1)*(1-t)^(n-i)*ctrl_pt(i);
% u = u + ctrl_pt(i) * nchoosek(n,i) * (t^i .* (1-t).^(n-i));?
end
end
\ No newline at end of file
function [t_out, z_out, u_out] =simulate_leg(z0,p,ground,tf,ctrl1,ctrl2,showPlot)
function [t_out, z_out, u_out] =simulate_leg(z0,p,ground,tf,ctrl1,ctrl2)
%% to remove
......@@ -57,14 +57,7 @@ function [t_out, z_out, u_out] =simulate_leg(z0,p,ground,tf,ctrl1,ctrl2,showPlot
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
%Torque Control Law
......@@ -199,97 +192,39 @@ 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(1)<0*3.14/180 || z(1)>150*3.14/180 || z(2)<0*3.14/180 || z(2)>150*3.14/180
% 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
%%
% 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
% Initialize video
% myVideo = VideoWriter('myVideoFile'); %open video file
% myVideo.FrameRate = 10; %can adjust this, 5 - 10 works well for me
% open(myVideo);
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);
% q2_min = 50 * pi/ 180;
% C1 = z(2) - q2_min; % C gives distance away from constraint
% dC= z(6);
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);
minimumX=min(keypoints(1,:))-0.2;
maximumX=max(keypoints(1,:))+0.2;
axis([minimumX maximumX -.3 .1]);
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)]);
% q2_max = 150 * pi/ 180;
% C2 = q2_max-z(2); % C gives distance away from constraint
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)]);
% if mod(i,800)
% frame = getframe(gcf);
% writeVideo(myVideo, frame);
% end
% qdot = z(5:6);
% J = [1 0];
% A = A_leg(z,p);
% A=A(1:2,1:2);
% % size(A)
pause(.01)
end
% close(myVideo);
% 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)
% 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));
%% Perform Dynamic simulation
dt = tf/(N.ctrl-1);
num_step = N.ctrl;
z_out(:,1) = z0;
for i=1:num_step-1
t = t_out(i);
[dz,u] = dynamics(t, 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)=u;
end
end
%Torque Control Law
function tau = control_law_torque(t, z, p,ctrl1,ctrl2 ,tf)
tau1 = BezierCurve(ctrl1.T, t/tf);
tau2 = BezierCurve(ctrl2.T, t/tf);
tau =[tau1;tau2];
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;
dz = casadi.MX(zeros(8,1));
% Form dz
dz(1:4,1) = z(5:8);
dz(5:8,1) = 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 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)
% 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));
%% Perform Dynamic simulation
dt = tf/(N.ctrl-1);
num_step = N.ctrl;
z_out(:,1) = z0;
for i=1:num_step-1
t = t_out(i);
[dz,u] = dynamics(t, 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)=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);
tau =[tau1;tau2];
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;
dz = casadi.MX(zeros(8,1));
% Form dz
dz(1:4,1) = z(5:8);
dz(5:8,1) = qdd;
dz
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 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
......@@ -20,17 +20,18 @@ function [cineq ceq] = constraints(x,z0,p,ground,tf)
% provided using an anonymous function, just as we use anonymous
% functions with ode45().
numCtrlPoints=length(x)/2;
numCtrlPoints=(length(x)-4)/2;
z0=[z0(1) z0(2) z0(3) z0(4) x(1) x(2) x(3) x(4)];
ctrl1.tf = tf; % control time points
ctrl1.T = x(1:numCtrlPoints);
ctrl1.T = x(5:(4+numCtrlPoints));
ctrl2.tf = tf; % control time points
ctrl2.T = x(numCtrlPoints+1:end);
ctrl2.T = x((4+numCtrlPoints+1):end);
[t, z, u] = simulate_leg(z0,p,ground,tf,ctrl1,ctrl2,false); % run simulation
[t, z, u] = simulate_leg(z0,p,ground,tf,ctrl1,ctrl2); % run simulation
% [minTheta,minThetat] = min(z(2,:));
% [maxTheta,maxThetat] = max(z(2,:));
......
function f = objective(x,z0,p,ground,tf)
% Inputs:
% x - an array of decision variables.
% z0 - the initial state
% p - simulation parameters
%
% Outputs:
% f - scalar value of the function (to be minimized) evaluated for the
% provided values of the decision variables.
%
% Note: fmincon() requires a handle to an objective function that accepts
% exactly one input, the decision variables 'x', and returns exactly one
% output, the objective function value 'f'. It is convenient for this
% assignment to write an objective function which also accepts z0 and p
% (because they will be needed to evaluate the objective function).
% However, fmincon() will only pass in x; z0 and p will have to be
% provided using an anonymous function, just as we use anonymous
% functions with ode45().
numCtrlPoints=(length(x)-4)/2;
z0=[z0 x(1) x(1:4) x(1:4) x(1:4)];
ctrl1.tf = tf; % control time points
ctrl1.T = x(5:(4+numCtrlPoints));
ctrl2.tf = tf; % control time points
ctrl2.T = x((4+numCtrlPoints+1):end);
[t, z, u] = simulate_leg(z0,p,ground,tf,ctrl1,ctrl2,false); % run simulation
x_end= z(3,end);
f = -x_end; % negative of stride
% m=1;
% d=x_end;
% g=9.8;
% uu=u*u';
% CoT= E/(m*g*d)
f = -max(z(4,:)); % negative of height
% alternate objective functions:
% f = tf; % final time
% ctrl_t = linspace(0, ctrl.tf, 50);
% ctrl_pt_t = linspace(0, ctrl.tf, length(ctrl.T));
% n = length(ctrl_t);
% ctrl_input = zeros(1,n);
%
% for i=1:n
% ctrl_input(i) = BezierCurve(x(3:end),ctrl_t(i)/ctrl.tf);
% end
%
% f = sum(sum(ctrl_input)); % minimize T^2 integral
% size(ctrl_input);
% cc=ctrl_input*ctrl_input'/50;
% size(u);
% uu=u*u';
% f = uu;
end
\ No newline at end of file
......@@ -17,16 +17,19 @@ function f = objective(x,z0,p,ground,tf)
% provided using an anonymous function, just as we use anonymous
% functions with ode45().
numCtrlPoints=length(x)/2;
numCtrlPoints=(length(x)-4)/2;
z0=[z0(1) z0(2) z0(3) z0(4) x(1) x(2) x(3) x(4)];
ctrl1.tf = tf; % control time points
ctrl1.T = x(1:numCtrlPoints);
ctrl1.T = x(5:(4+numCtrlPoints));
ctrl2.tf = tf; % control time points
ctrl2.T = x(numCtrlPoints+1:end);
ctrl2.T = x((4+numCtrlPoints+1):end);
[t, z, u] = simulate_leg(z0,p,ground,tf,ctrl1,ctrl2,false); % run simulation
[t, z, u] = simulate_leg(z0,p,ground,tf,ctrl1,ctrl2); % run simulation
x_end= z(3,end);
......
function animate_solution(t_out, z_out, u_out, p, ground, ctrl1, ctrl2, saveVideo)
% clf;
% plot_control(ctrl1,u_out(1,:),1)
% clf;
% plot_control(ctrl2,u_out(2,:),4)
%% Animate Solution
figure(6); clf;
hold on
plot([-2 2],[ground.ground_height ground.ground_height],'k');
animateSol(t_out, z_out,p,saveVideo);
end
function animateSol(tspan, x,p,saveVideo)
% Prepare plot handles
hold on
if saveVideo
%Initialize video
myVideo = VideoWriter('myVideoFile'); %open video file
myVideo.FrameRate = 10; %can adjust this, 5 - 10 works well for me
open(myVideo);
end
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],[