匿名用户
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= ...