Skip to content

Commit

Permalink
traj
Browse files Browse the repository at this point in the history
  • Loading branch information
骏文 钟 committed Apr 5, 2019
1 parent 7f1ab2b commit 6939535
Show file tree
Hide file tree
Showing 29 changed files with 1,317 additions and 73 deletions.
8 changes: 5 additions & 3 deletions 梁斌-空间机器人：建模、规划与控制/Ikine_Step.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,9 @@
Angle = [0 0 0 0 0 0];

s3 = -((px - d6*ax)^2 + (py - d6*ay)^2 + (pz - d6*az - d1)^2 - a2^2 - d4^2) / (2*a2*d4);
c3 = real(sqrt(1 - s3^2));
angle3 = asind(s3);
% c3 = real(sqrt(1 - s3^2));
c3 = cosd(angle3);
A = a2 - d4*s3;
B = d4*c3;
C = d1 - pz + d6*az;
Expand All @@ -48,8 +50,8 @@
angle1 = atan2d( (py - d6*ay)*(a2*c2 - d4*s23) , (px - d6*ax)*(a2*c2 - d4*s23) );
for r3 = 1:2%loop3
c1 = cosd(angle1);
s1 = real(sqrt(1 - c1^2));
c23 = real(sqrt(1 - s23^2));
s1 = sind(angle1);
c23 = cosd(angle2 + angle3);
ax36 = ax*c1*c23 - az*s23 + ay*c23*s1;
ay36 = ax*s1 - ay*c1;
az36 = -az*c23 - ax*c1*s23 - ay*s1*s23;
Expand Down
8 changes: 5 additions & 3 deletions 梁斌-空间机器人：建模、规划与控制/SCurve3/Ikine_Step.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,9 @@
Angle = [0 0 0 0 0 0];

s3 = -((px - d6*ax)^2 + (py - d6*ay)^2 + (pz - d6*az - d1)^2 - a2^2 - d4^2) / (2*a2*d4);
c3 = real(sqrt(1 - s3^2));
angle3 = asind(s3);
% c3 = real(sqrt(1 - s3^2));
c3 = cosd(angle3);
A = a2 - d4*s3;
B = d4*c3;
C = d1 - pz + d6*az;
Expand All @@ -48,8 +50,8 @@
angle1 = atan2d( (py - d6*ay)*(a2*c2 - d4*s23) , (px - d6*ax)*(a2*c2 - d4*s23) );
for r3 = 1:2%loop3
c1 = cosd(angle1);
s1 = real(sqrt(1 - c1^2));
c23 = real(sqrt(1 - s23^2));
s1 = sind(angle1);
c23 = cosd(angle2 + angle3);
ax36 = ax*c1*c23 - az*s23 + ay*c23*s1;
ay36 = ax*s1 - ay*c1;
az36 = -az*c23 - ax*c1*s23 - ay*s1*s23;
Expand Down
16 changes: 8 additions & 8 deletions 梁斌-空间机器人：建模、规划与控制/SCurve3/main.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@

%平移
trans = [1 0 0 0.1;
0 1 0 -0.2;
0 1 0 0.2;
0 0 1 -0.3;
0 0 0 1;];
pose_end = trans*pose_start
Expand All @@ -40,7 +40,7 @@

v = 0.1;%运动速度0.1m/s
a = 0.03;%加速度 0.01接近三角函数
t = 0.1;%插补周期10ms（plc周期）
t = 0.01;%插补周期10ms（plc周期）
L = sqrt(trans(1,4)^2 + trans(2,4)^2 + trans(3,4)^2);%distance
N = ceil(L/(v*t)) + 1;%插补数量

Expand All @@ -57,10 +57,10 @@
% plot3(x,y,z,'r'),xlabel('x'),ylabel('y'),zlabel('z'),hold on,plot3(x,y,z,'o','color','g'),grid on;

for i = 1:N
pose_end(1,4) = x(i);
pose_end(2,4) = y(i);
pose_end(3,4) = z(i);
q(i,:) = Ikine_Step(pose_end,Q_zero);%反解
pose_start(1,4) = x(i);
pose_start(2,4) = y(i);
pose_start(3,4) = z(i);
q(i,:) = Ikine_Step(pose_start,Q_zero);%反解
end

figure(2);
Expand All @@ -80,8 +80,8 @@
end

figure(3);
% plot3(x(1,:),y(1,:),z(1,:),'color',[1,0,0],'LineWidth',2);
plot3(x,y,z,'r'),xlabel('x'),ylabel('y'),zlabel('z'),hold on,plot3(x,y,z,'o','color','g'),grid on;
plot3(x(1,:),y(1,:),z(1,:),'color',[1,0,0],'LineWidth',2);
% plot3(x,y,z,'r'),xlabel('x'),ylabel('y'),zlabel('z'),hold on,plot3(x,y,z,'o','color','g'),grid on;
q1 = zeros(N,8);%位置-->各关节为弧度值,8个单独关节运动.

for i = 1:N
Expand Down
8 changes: 5 additions & 3 deletions 梁斌-空间机器人：建模、规划与控制/SCurveNew/Ikine_Step.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,9 @@
Angle = [0 0 0 0 0 0];

s3 = -((px - d6*ax)^2 + (py - d6*ay)^2 + (pz - d6*az - d1)^2 - a2^2 - d4^2) / (2*a2*d4);
c3 = real(sqrt(1 - s3^2));
angle3 = asind(s3);
% c3 = real(sqrt(1 - s3^2));
c3 = cosd(angle3);
A = a2 - d4*s3;
B = d4*c3;
C = d1 - pz + d6*az;
Expand All @@ -48,8 +50,8 @@
angle1 = atan2d( (py - d6*ay)*(a2*c2 - d4*s23) , (px - d6*ax)*(a2*c2 - d4*s23) );
for r3 = 1:2%loop3
c1 = cosd(angle1);
s1 = real(sqrt(1 - c1^2));
c23 = real(sqrt(1 - s23^2));
s1 = sind(angle1);
c23 = cosd(angle2 + angle3);
ax36 = ax*c1*c23 - az*s23 + ay*c23*s1;
ay36 = ax*s1 - ay*c1;
az36 = -az*c23 - ax*c1*s23 - ay*s1*s23;
Expand Down
4 changes: 2 additions & 2 deletions 梁斌-空间机器人：建模、规划与控制/SCurveNew/main.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -105,8 +105,8 @@

figure(3);
plot3(x(1,:),y(1,:),z(1,:),'color',[1,0,0],'LineWidth',2);
hold on;
plot3(x(1,:),y(1,:),z(1,:),'o','color','g'),grid on;
% hold on;
% plot3(x(1,:),y(1,:),z(1,:),'o','color','g'),grid on;

q1 = zeros(length(q),8);%位置-->各关节为弧度值,8个单独关节运动.

Expand Down
64 changes: 38 additions & 26 deletions 梁斌-空间机器人：建模、规划与控制/main.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,38 +2,50 @@
clc;
%建立机器人模型
% theta d a alpha offset
% SL1=Link([0 0.28 0 -pi/2 0 ],'standard');
% SL2=Link([0 0 0.34966093 0 0 ],'standard');
% SL3=Link([0 0 0 -pi/2 0 ],'standard');
% SL4=Link([0 0.35014205 0 pi/2 0 ],'standard');
% SL5=Link([0 0 0 -pi/2 0 ],'standard');
% SL6=Link([0 0.0745 0 0 0 ],'standard');
% SL2.offset = -pi/2;
% robot=SerialLink([SL1 SL2 SL3 SL4 SL5 SL6],'name','standard');

robot = importrobot('Test_Link6.urdf');%导入机器人描述格式
robot.DataFormat='column';%数据格式为列，row为行；为‘row'时q0要转置-->q0’
robot.Gravity = [0 0 -9.80];%重力方向设置

q1 = [0; 0; 0; 0; 0; 0; 0; 0];%位置-->各关节为弧度值,8个单独关节运动.
show(robot,q1,'PreservePlot',false);
SL1=Link([0 0.28 0 -pi/2 0 ],'standard');
SL2=Link([0 0 0.34966093 0 0 ],'standard');
SL3=Link([0 0 0 -pi/2 0 ],'standard');
SL4=Link([0 0.35014205 0 pi/2 0 ],'standard');
SL5=Link([0 0 0 -pi/2 0 ],'standard');
SL6=Link([0 0.0745 0 0 0 ],'standard');
SL2.offset = -pi/2;
robot = SerialLink([SL1 SL2 SL3 SL4 SL5 SL6],'name','SD700');
robot1 = SerialLink([SL1 SL2 SL3 SL4 SL5 SL6],'name','standard');
% robot = importrobot('Test_Link6.urdf');%导入机器人描述格式
% robot.DataFormat='column';%数据格式为列，row为行；为‘row'时q0要转置-->q0’
% robot.Gravity = [0 0 -9.80];%重力方向设置
%
% q1 = [0; 0; 0; 0; 0; 0; 0; 0];%位置-->各关节为弧度值,8个单独关节运动.
% show(robot,q1,'PreservePlot',false);

% view([150 6]);%figure大小设置
% axis([-0.6 0.6 -0.6 0.6 0 1.35]);%坐标范围
% camva('auto');%设置相机视角
% daspect([1 1 1]);%控制沿每个轴的数据单位长度,要在所有方向上采用相同的数据单位长度，使用 [1 1 1]


% Q_zero1 = [0,0,0,0,0,0];%底座 -> 抓手
% Angle_Last = Q_zero1 + [0,90,0,0,0,0];
% p1 = Fkine_Final(Q_zero1)%正解
% q1 = Ikine_Step(p1,Q_zero1)%反解
% q1 = q1.*[pi/180,pi/180,pi/180,pi/180,pi/180,pi/180];
% robot.plot(q1);
Q_zero1 = [0,0,0,0,90,0];%底座 -> 抓手
Angle_Last = Q_zero1 + [0,90,0,0,0,0];
p1 = Fkine_Final(Q_zero1)%正解
%平移
trans = [1 0 0 0.1;
0 1 0 -0.2;
0 0 1 -0.3;
0 0 0 1;];
pose_end = trans*p1;
% pose_end = p1;

q1 = Ikine_Step(pose_end,Q_zero1)%反解
q2 = q1.*[pi/180,pi/180,pi/180,pi/180,pi/180,pi/180];
figure(1),robot.plot(q2);


% 转弧度
Q_zero = Q_zero1.* [pi/180,pi/180,pi/180,pi/180,pi/180,pi/180];
p = robot1.fkine(Q_zero);
%平移
p.t = [0.45014205;-0.2;0.25516093];

%转弧度
% Q_zero = Q_zero1.* [pi/180,pi/180,pi/180,pi/180,pi/180,pi/180];
% p = robot.fkine(Q_zero);
% q = robot.ikine(p)
% robot.plot(q);
q = robot1.ikine(pose_end);
figure(2),robot1.plot(q);
q3 = q.*[180/pi,180/pi,180/pi,180/pi,180/pi,180/pi]
Binary file added 空间圆弧/1.bmp
View file
Binary file not shown.
Binary file added 空间圆弧/2.bmp
View file
Binary file not shown.
35 changes: 20 additions & 15 deletions 空间圆弧/CircleMain.m
View file
Original file line number Diff line number Diff line change
@@ -1,14 +1,6 @@
clc;
clear;
p1 = [0,0,0];
p2 = [1,0.7,0];
p3 = [2,0,0];
[pc,r] = CircleCenter(p1,p2,p3);
sumStep = 100;%插补数量
function p_i = CircleMain(p1,p2,p3,v_l,a_l,t)

v = 0.1;%运动速度0.1m/s
a = 0.03;%加速度 0.01接近三角函数
s = linemove(0,1,v,a,sumStep);
[pc,r] = CircleCenter(p1,p2,p3);

% 建立圆弧坐标系
% 计算转换矩阵
Expand Down Expand Up @@ -39,18 +31,31 @@
theta12 = atan2(q2(2),q2(1));
end


% v = 0.1;%运动速度0.1m/s
% a = 0.03;%加速度 0.01接近三角函数
% t = 0.01;%插补周期10ms（plc周期）
L = theta13*r;%distance
N = ceil(L/(v_l*t)) + 1;%插补数量

sumStep = N;%插补数量
s = linemove(0,1,v_l,a_l,sumStep);

% 轨迹插补
for count = 1:sumStep
p_i(:,count) = T*[r*cos(s(count)*theta13) r*sin(s(count)*theta13) 0 1]';
p(:,count) = T*[r*cos(s(count)*theta13) r*sin(s(count)*theta13) 0 1]';
end
count =1;
p_i = zeros(3,N);
p_i = p(1:3,:);
% count =1;
%线性
% for step = 0:theta13/sumStep: theta13
% p_i1(:,count) = T*[r*cos(step) r*sin(step) 0 1]';
% count = count+1;
% end
figure(2);
plot3(p_i(1,:),p_i(2,:),p_i(3,:),'r'),xlabel('x'),ylabel('y'),zlabel('z'),hold on,plot3(p_i(1,:),p_i(2,:),p_i(3,:),'o','color','g'),grid on;
% figure(2);
% plot3(p_i(1,:),p_i(2,:),p_i(3,:),'r'),xlabel('x'),ylabel('y'),zlabel('z'),hold on,plot3(p_i(1,:),p_i(2,:),p_i(3,:),'o','color','g'),grid on;
% hold on;
% plot3(p_i1(1,:),p_i1(2,:),p_i1(3,:),'r'),xlabel('x'),ylabel('y'),zlabel('z'),hold on,plot3(p_i1(1,:),p_i1(2,:),p_i1(3,:),'o','color','r'),grid on;
axis([0 2 0 2]);


56 changes: 56 additions & 0 deletions 空间圆弧/Fkine_Final.m
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,56 @@
function T = Fkine_Final(Theta)

%D-H ²ÎÊý
d1 = 0.28;
d4 = 0.35014205;
d6 = 0.0745;
a2 = 0.34966093;
Theta(2) = Theta(2) - 90;%offset

%¼ÆËãÈý½Çº¯Êý
s1 = sind(Theta(1));
c1 = cosd(Theta(1));

s2 = sind(Theta(2));
c2 = cosd(Theta(2));

s23 = sind(Theta(2)+Theta(3));
c23 = cosd(Theta(2)+Theta(3));

s4 = sind(Theta(4));
c4 = cosd(Theta(4));

s5 = sind(Theta(5));
c5 = cosd(Theta(5));

s6 = sind(Theta(6));
c6 = cosd(Theta(6));

a = c23*(c4*c5*c6 - s4*s6) - s23*s5*c6;
b = c23*(-c4*c5*s6 - s4*c6) + s23*s5*s6;
c = -c23*c4*s5 - s23*c5;
d = -c23*c4*s5*d6 - s23*(c5*d6 + d4) + a2*c2;
e = s23*(c4*c5*c6 - s4*s6) + c23*s5*c6;
f = s23*(-c4*c5*s6 - s4*c6) - c23*s5*s6;
g = -s23*c4*s5 + c23*c5;
h = -s23*c4*s5*d6+c23*(c5*d6 + d4) + a2*s2;
i = -s4*c5*c6 - c4*s6;
j = s4*c5*s6 - c4*c6;
k = s4*s5;
l = s4*s5*d6;


T(1,1) = c1*a - s1*i;
T(1,2) = c1*b - s1*j;
T(1,3) = c1*c - s1*k;
T(1,4) = c1*d - s1*l;
T(2,1) = s1*a + c1*i;
T(2,2) = s1*b + c1*j;
T(2,3) = s1*c + c1*k;
T(2,4) = s1*d + c1*l;
T(3,1) = -e;
T(3,2) = -f;
T(3,3) = -g;
T(3,4) = -h+d1;
T(4,4) = 1;

Loading

0 comments on commit 6939535

Please sign in to comment.