四足机器狗建模——单腿matlab 建模（示教，三维）

matlab 2023

坐标系： B，0,1,2,3 ，4

4 为足端坐标系。

三自由度，可计算但不考虑末端姿态。

---

轨迹规划非严格

clear 
clc
%注：以右前腿为例，电机ID分别为0,1,2
%设定初始化参数
m_l1 = 200;m_l2 = 200;  %大腿长。小腿长。旋转轴到旋转轴，旋转轴到足心
m_d1 = 100;  %关节2的连杆偏距
%角度转弧度？ deg2rad(30)
m_theta_init_1 = -30/ 180 * pi;  %负的，电机1.2在初始零位时朝内侧的旋转角度，相对于水平
m_theta_init_2 = -60/ 180 * pi;  %负的，大腿杆线方向与重力方向夹角
m_theta_init_3 =  150/ 180 * pi; %正的，大腿杆线方向与小腿杆线方向的夹角，取外侧夹角，一般为钝角

theta1  = m_theta_init_1;
theta2  = m_theta_init_2;
theta3  = m_theta_init_3;
theta4  = 0;%不考虑足端姿态

theta1  = deg2rad(-45);
theta2  = deg2rad(45);
theta3  = deg2rad(45);
theta4  = 0;%不考虑足端姿态



%建立DH参数  
L1 = Link('d', 0, 'a', 0, 'alpha', 0  ,'modified');  %髋关节俯仰
L2 = Link( 'd', m_d1, 'a', 0, 'alpha', 90/180*pi ,'modified');  %髋关节横滚
L3 = Link( 'd', 0, 'a', m_l1, 'alpha', 0 ,'modified'); %膝关节横滚
L4 = Link( 'd', 0, 'a', m_l2, 'alpha', 0 ,'modified'); %足关节
% %关节限制
L(1).qlim=[-pi/6/pi*180,pi/3/pi*180];
L(2).qlim=[-pi*4/3/pi*180,pi/1.3/pi*180];
L(3).qlim=[pi/6/pi*180,pi*(5/6)/pi*180];
L(4).qlim=[-pi/pi*180,pi/pi*180];
%连成机器人
leg = SerialLink([L1 L2 L3 L4], 'name', 'Leg');
%leg = SerialLink([L1 L2 L3 ], 'name', 'Leg');
% 打印mod_DH机器人模型
leg.display();

% 设置关节初始位置
init_pos = [theta1,theta2,theta3,theta4];
syms q1 q2 q3 q4 real;
init_pos1 = [theta1,theta2,theta3,];

%输出三维
leg.plot(init_pos);

%示教
leg.teach();

%获取机器人关节角
theta1 = L1.theta;

%机器人运动学
T = leg.fkine(init_pos);
disp("末端执行器位姿：");
disp(T);

% 计算雅可比矩阵
J0 = leg.jacob0(init_pos);%关节速度映射到笛卡尔
disp("雅可比矩阵0：");
disp(J0);

Je= leg.jacobe(init_pos);%关节速度映射到笛卡尔
disp("雅可比矩阵e：");
%disp(Je)


init_pos_car = [
   0 ,        0  ,       1 ,        -12.11;
   0 ,        1  ,       0 ,        -108.5;
   1   ,      0 ,        0,        93.83;
   0  ,       0 ,        0 ,        1;
];
%init_pos_car = transl(12,12,12)
%机器人逆运动学
% Ti = leg.ikine(init_pos_car);
% disp("逆运动学：");
% disp(Ti);


% R40 = [         0 ,  43.7527, -153.2089;
%   -12.1091 ,  46.9139 ,  64.2788;
%  -108.4789 ,  81.2573,  111.3341;];
% R40J = [        0 ,  43.7527, -153.2089,0,0,0;
%   -12.1091 ,  46.9139 ,  64.2788,0,0,0;
%  -108.4789 ,  81.2573,  111.3341,0,0,0;
%  0,0,0,        0 ,  43.7527, -153.2089;
%     0,0,0,         -12.1091 ,  46.9139 ,  64.2788;
%    0,0,0,           -108.4789 ,  81.2573,  111.3341;
% ];
% J2 = R40J*Je;
% disp(J2)

%t = linspace(0,2,51); %等价于==0:0.04:2        %0-2s,进行51次插值
%[P,dP,ddP]=tpoly(0,3,t);

% 指定初末速度
%[P,dP,ddP]=tpoly(0,3,51,0.02,0.01);
% 0-3s内进行51次插值
%%

P1=[50,50,50];
P2=[250,-50,-50];
t=linspace(0,2,51);
Traj = mtraj(@tpoly,P1,P2,t);
n = size(Traj,1);
T = zeros(4,4,n);
for i=1:n
    T(:,:,i) = transl(Traj(i,:))*trotx(0);
end

Qtraj  = leg.ikunc(T);
%leg.plot(Qtraj);                   %只运动，不显示轨迹
leg.plot(Qtraj,'trail','b');           %录制轨迹
% % Five_dof.plot(Qtraj,'trail','b','movie','trail.gif');       %保存录像
%% 
hold on
plot(t,Traj(:,1),'.-',LineWidth=1)
plot(t,Traj(:,2),'.-',LineWidth=1)
plot(t,Traj(:,3),'.-',LineWidth=1)

grid on
legend('x','y','z');
xlabel('time');
ylabel('position');




% %B站
% T1 = transl(100,100,100)*trotx(150);
% T2 = transl(-100,100,100)*trotx(150);
% 
% q1  = leg.ikunc(T1);
% q2  = leg.ikunc(T2);
% 
% leg.plot(q1); 
% pause;
% leg.plot(q2);