匿名用戶

1級

2016-05-25 回答

The following is a function I wrote to generate a stick diagram of robot motion. Hope it is helpful to you all.

function = stick(filename,user_frame_per_second,max_step)

global robot

foot2=;

mov_traj = load(filename);

dt = mov_traj(2,1) - mov_traj(1,1);

frame_per_second = fix(1/dt);

if user_frame_per_second == 0

show_frame_per_second = frame_per_second;

else

show_frame_per_second = user_frame_per_second;

end

capture_frame_per_second = 20;

color0 = ;

color1 = ;

color2 = ;

color3 = *0.5;

color4 = *0.5;

width = 2;

scrsz = get(0,'ScreenSize');

animation = figure;

%hold on

xxlim =

yylim =

set(animation,'name','2D walking','Position',);

h_axes = axes('Parent',animation,'Units','Pixels','Position',,'Ylim',yylim,'Xlim',xxlim);

%tx = text('Parent',h_axes,'Visible','off');

set(h_axes,'visible','on');

ground = line('Parent',h_axes,'Color',,'Visible','off','LineWidth',2);

set(ground, 'Parent',h_axes,'Xdata',xxlim,'Ydata', ,'visible','on');

idx =1

ss_flag = 0;

step_count = 0;

l_ankle_pos = zeros(2,1); l_knee_pos = zeros(2,1); l_hip_pos = zeros(2,1); head_pos = zeros(2,1);

r_knee_pos = zeros(2,1); r_ankle_pos = zeros(2,1);

r_ankle_pos_b_save = ';

base = ';

for i = 1:size(mov_traj,1)

tt = mov_traj(i,1);

l_ankle_abs = mov_traj(i,2);

l_knee_abs = l_ankle_abs + mov_traj(i,3);

l_hip_abs = l_knee_abs + mov_traj(i,4);

r_hip_abs = l_hip_abs + mov_traj(i,5);

r_knee_abs = r_hip_abs + mov_traj(i,6);

% r_knee_abs = r_knee_abs;

l_ankle_pos_b =';

l_knee_pos_b = ';

l_hip_pos_b = ';

head_pos_b = ';

r_knee_pos_b = ';

r_ankle_pos_b = ';

if (r_ankle_pos_b_save(1) > 0 && r_ankle_pos_b(1) < 0) % switch

step_count = step_count +1;

if step_count >= max_step

break

end

ss_flag = 1;

tmp = color0; color0 = color4; color4=tmp;

tmp = color1; color1 = color3; color3=tmp;

set(l_calf,'Color',color0,'Visible','off');

set(l_thigh,'Color',color1,'Visible','off');

set(torso,'Color',color2,'Visible','off')

set(r_thigh,'Color',color3,'Visible','off');

set(r_calf,'Color',color4,'Visible','off');

% hip_travel = hip_travel + r_ankle_pos_b_save(1);

% ground_height = ground_height + r_ankle_pos_b_save(2);

base = base + r_ankle_pos_b_save;

%ground_height =0 ;

%set(h_axes,'Ylim',);

base;

r_ankle_pos_b_save;

r_ankle_pos_b;

%pause

end

r_ankle_pos_b_save = r_ankle_pos_b;

% drift = l_hip_pos(1);

% l_ankle_pos(1) = l_ankle_pos(1) - drift;

% l_knee_pos(1) = l_knee_pos(1) - drift;

% l_hip_pos(1) = l_hip_pos(1) - drift;

% head_pos(1) = head_pos(1) - drift;

% r_knee_pos(1) = r_knee_pos(1) - drift;

% r_ankle_pos(1) = r_ankle_pos(1) - drift;

l_ankle_pos = l_ankle_pos_b + base;

l_knee_pos = l_knee_pos_b + base;

l_hip_pos = l_hip_pos_b + base;

head_pos = head_pos_b + base;

r_knee_pos = r_knee_pos_b + base;

r_ankle_pos = r_ankle_pos_b + base;

foot2 = ;

if (mod(i,fix(frame_per_second/show_frame_per_second)) == 1 )

torso = line('Parent',h_axes,'Color',color2,'Visible','off','LineWidth',width);

r_thigh = line('Parent',h_axes,'Color',color3,'Visible','off','LineWidth',width);

r_calf = line('Parent',h_axes,'Color',color4,'Visible','off','LineWidth',width);

l_calf = line('Parent',h_axes,'Color',color0,'Visible','off','LineWidth',width);

l_thigh = line('Parent',h_axes,'Color',color1,'Visible','off','LineWidth',width);

if mod(step_count,2) == 1

set(l_calf, 'Parent',h_axes,'Xdata',,'Ydata', ,'visible','on');

set(l_thigh, 'Parent',h_axes,'Xdata',,'Ydata', ,'visible','on');

set(torso, 'Parent',h_axes,'Xdata',,'Ydata', ,'visible','on');

set(r_thigh, 'Parent',h_axes,'Xdata',,'Ydata', ,'visible','on');

set(r_calf, 'Parent',h_axes,'Xdata',,'Ydata', ,'visible','on');

else

set(torso, 'Parent',h_axes,'Xdata',,'Ydata', ,'visible','on');

set(r_thigh, 'Parent',h_axes,'Xdata',,'Ydata', ,'visible','on');

set(r_calf, 'Parent',h_axes,'Xdata',,'Ydata', ,'visible','on');

set(l_calf, 'Parent',h_axes,'Xdata',,'Ydata', ,'visible','on');

set(l_thigh, 'Parent',h_axes,'Xdata',,'Ydata', ,'visible','on');

end

stime=sprintf('time:%5.2f',tt);

%set(tx,'Parent',h_axes,'Position',,'String',stime,'visible','on');

%set(h_axes,'Xlim',);

drawnow;

end

if l_ankle_pos(1) > xxlim(2)

break

end

end

xlabel('(m)')

ylabel('(m)')

zbyoyicc (站內聯系TA)

7樓: Originally posted by blackwave at 2015-01-15 11:38:18

The following is a function I wrote to generate a stick diagram of robot motion. Hope it is helpful to you all.

function = stick(filename,user_frame_per_second,max_step)

global robot

foot2= ...