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

first learnt hop

parent 54ad5984
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;
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;
function [t_out, z_out, u_out] =simulate_leg(z0,p,ground,tf,ctrl1,ctrl2,showPlot)
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]';
%% 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;
......@@ -56,7 +29,7 @@ function [t_out, z_out, u_out] =simulate_leg(z0,tf,ctrl1,ctrl2)
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;
......@@ -75,20 +48,23 @@ function [t_out, z_out, u_out] =simulate_leg(z0,tf,ctrl1,ctrl2)
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');
%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
%Torque Control Law
......@@ -186,8 +162,11 @@ function [dz,tau] = dynamics(t,z,p,ctrl1,ctrl2)
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);
......@@ -217,11 +196,11 @@ 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
% 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
......@@ -259,7 +238,7 @@ function animateSol(tspan, x,p)
h_title = title('t=0.0s');
axis equal
axis([-.2 .2 -.3 .1]);
% axis([-.2 .2 -.3 .1]);
%Step through and update animation
for i = 1:length(tspan)
......@@ -277,6 +256,10 @@ function animateSol(tspan, x,p)
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
......
......@@ -62,7 +62,7 @@ function [t_out, z_out, u_out] =simulate_leg(z0,p,ground,tf,ctrl1,ctrl2,showPlot
figure(6); clf;
hold on
plot([-.2 .2],[ground.ground_height ground.ground_height],'k');
plot([-2 2],[ground.ground_height ground.ground_height],'k');
animateSol(tspan, z_out,p);
end
end
......@@ -228,6 +228,13 @@ 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);
......@@ -256,6 +263,10 @@ function animateSol(tspan, x,p)
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
......@@ -270,9 +281,15 @@ 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
pause(.01)
end
close(myVideo);
end
......@@ -18,7 +18,6 @@ function f = objective(x,z0,p,ground,tf)
% functions with ode45().
numCtrlPoints=5;
ctrl1.tf = tf; % control time points
ctrl1.T = x(1:numCtrlPoints);
......
......@@ -25,7 +25,7 @@ function plot_control(ctrl,u,figNum)
ylabel('torque (Nm)')
title('Control Input Trajectory')
filename=strcat('./img/',prob,num2str(id),'_cntrl.png');
filename=strcat('./img/',prob,num2str(figNum),'_cntrl.png');
fig=gcf;
saveas(fig,filename);
end
\ No newline at end of file
img/6.2_cntrl.png

36.4 KB | W: | H:

img/6.2_cntrl.png

37.3 KB | W: | H:

img/6.2_cntrl.png
img/6.2_cntrl.png
img/6.2_cntrl.png
img/6.2_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