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

first learnt hop

parent 955699ea
function [t_out, z_out, u_out] =simulate_leg(z0,p,ground,tf,ctrl1,ctrl2,showPlot)
%% 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--');
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
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 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
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);
minimumX=min(keypoints(1,:))-0.2
maximumX=max(keypoints(1,:))+0.2
axis([minimumX .2 -.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)]);
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
......@@ -230,9 +230,9 @@ function animateSol(tspan, x,p)
hold on
% Initialize video
myVideo = VideoWriter('myVideoFile'); %open video file
myVideo.FrameRate = 10; %can adjust this, 5 - 10 works well for me
open(myVideo);
% 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);
......@@ -282,14 +282,14 @@ function animateSol(tspan, x,p)
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
% if mod(i,800)
% frame = getframe(gcf);
% writeVideo(myVideo, frame);
% end
pause(.01)
end
close(myVideo);
% close(myVideo);
end
function [cineq ceq] = constraints(x,z0,p,ground,tf)
% Inputs:
% x - an array of decision variables.
% z0 - the initial state
% p - simulation parameters
%
% Outputs:
% cineq - an array of values of nonlinear inequality constraint functions.
% The constraints are satisfied when these values are less than zero.
% ceq - an array of values of nonlinear equality constraint functions.
% The constraints are satisfied when these values are equal to zero.
%
% Note: fmincon() requires a handle to an constraint function that accepts
% exactly one input, the decision variables 'x', and returns exactly two
% outputs, the values of the inequality constraint functions 'cineq' and
% the values of the equality constraint functions 'ceq'. It is convenient
% in this case 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=5;
ctrl1.tf = tf; % control time points
ctrl1.T = x(1:numCtrlPoints);
ctrl2.tf = tf; % control time points
ctrl2.T = x(numCtrlPoints+1:end);
[t, z, u] = simulate_leg(z0,p,ground,tf,ctrl1,ctrl2); % run simulation
% [minTheta,minThetat] = min(z(2,:));
% [maxTheta,maxThetat] = max(z(2,:));
%
%
% cineq = [-minTheta,maxTheta-3.14159/2.0];
%
% cineq1 = -min(z(2,:));
% cineq2 = max(z(2,:))-3.14159/2;
% cineq = [cineq1, cineq2];
% ceq = [x(2) - t(indices(1))]; % prob 5
%
%
% COM = COM_jumping_leg(z,p);
%
% [maxy,maxyI] = max(COM(2,:));
% vel=max(COM(4,maxyI));
%
% ceq = [x(2) - t(indices(1)), maxy-0.4,vel]; % prob 6
cineq=[];
q1_start=z0(1) ;
q2_start=z0(2) ;
x_start= z0(3);
y_start=z0(4);
dq1_start=z0(5) ;
dq2_start=z0(6) ;
dx_start= z0(7);
dy_start=z0(8);
q1_end=z0(1) ;
q2_end=z0(2) ;
x_end= z0(3);
y_end=z0(4);
dq1_end=z0(5) ;
dq2_end=z0(6) ;
dx_end= z0(7);
dy_end=z0(8);
ceq=[q1_start-q1_end,q2_start-q2_end,x_start-x_sta];
% simply comment out any alternate constraints when not in use
end
\ No newline at end of file
......@@ -20,7 +20,7 @@ function [cineq ceq] = constraints(x,z0,p,ground,tf)
% provided using an anonymous function, just as we use anonymous
% functions with ode45().
numCtrlPoints=5;
numCtrlPoints=length(x)/2;
ctrl1.tf = tf; % control time points
......
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=5;
ctrl1.tf = tf; % control time points
ctrl1.T = x(1:numCtrlPoints);
ctrl2.tf = tf; % control time points
ctrl2.T = x(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 height
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,7 +17,7 @@ function f = objective(x,z0,p,ground,tf)
% provided using an anonymous function, just as we use anonymous
% functions with ode45().
numCtrlPoints=5;
numCtrlPoints=length(x)/2;
ctrl1.tf = tf; % control time points
ctrl1.T = x(1:numCtrlPoints);
......@@ -31,7 +31,13 @@ function f = objective(x,z0,p,ground,tf)
x_end= z(3,end);
f = -x_end; % negative of height
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
......
File added
img/6.1_cntrl.png

27.1 KB | W: | H:

img/6.1_cntrl.png

26.3 KB | W: | H:

img/6.1_cntrl.png
img/6.1_cntrl.png
img/6.1_cntrl.png
img/6.1_cntrl.png
  • 2-up
  • Swipe
  • Onion skin
img/6.3_cntrl.png

33.6 KB | W: | H:

img/6.3_cntrl.png

35.4 KB | W: | H:

img/6.3_cntrl.png
img/6.3_cntrl.png
img/6.3_cntrl.png
img/6.3_cntrl.png
  • 2-up
  • Swipe
  • Onion skin
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