Matlab与V-rep联合仿真-姿态解算

结构参数获取

参数解释

在这里插入图片描述

b=0.094

在这里插入图片描述

w=0.2641

在这里插入图片描述

l=0.366

在这里插入图片描述

代码

主函数

%通讯初始化 从simpleTest中copy过来
clear
clc
disp('Program started');
vrep=remApi('remoteApi'); 
vrep.simxFinish(-1); 
clientID=vrep.simxStart('127.0.0.1',19999,true,true,5000,5);  %如果返回值为-1 则代表通讯不成功,返回值为0代表通讯成功
disp(clientID);
%%
if (clientID>-1)
   disp('Connected to remote API server');
   vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot); %开启仿真
   %声明节点,关节初始化,12个关节
 [rec ,RR_calf_joint]=vrep.simxGetObjectHandle (clientID,'RR_calf_joint',vrep.simx_opmode_blocking);
 [rec ,FR_calf_joint]=vrep.simxGetObjectHandle (clientID,'FR_calf_joint',vrep.simx_opmode_blocking);
 [rec ,RR_thigh_joint]=vrep.simxGetObjectHandle (clientID,'RR_thigh_joint',vrep.simx_opmode_blocking);
 [rec ,FR_thigh_joint]=vrep.simxGetObjectHandle (clientID,'FR_thigh_joint',vrep.simx_opmode_blocking);
 [rec ,RR_hip_joint]=vrep.simxGetObjectHandle (clientID,'RR_hip_joint',vrep.simx_opmode_blocking);
 [rec ,FR_hip_joint]=vrep.simxGetObjectHandle (clientID,'FR_hip_joint',vrep.simx_opmode_blocking);
 [rec ,RL_calf_joint]=vrep.simxGetObjectHandle (clientID,'RL_calf_joint',vrep.simx_opmode_blocking);
 [rec ,FL_calf_joint]=vrep.simxGetObjectHandle (clientID,'FL_calf_joint',vrep.simx_opmode_blocking);
 [rec ,RL_thigh_joint]=vrep.simxGetObjectHandle (clientID,'RL_thigh_joint',vrep.simx_opmode_blocking);
 [rec ,FL_thigh_joint]=vrep.simxGetObjectHandle (clientID,'FL_thigh_joint',vrep.simx_opmode_blocking);
 [rec ,RL_hip_joint]=vrep.simxGetObjectHandle (clientID,'RL_hip_joint',vrep.simx_opmode_blocking);
 [rec ,FL_hip_joint]=vrep.simxGetObjectHandle (clientID,'FL_hip_joint',vrep.simx_opmode_blocking);
 %12个电机力矩参数
 RR_hip_joint_force=500;     RR_thigh_joint_force=500;         RR_calf_joint_force=500; %第一条腿
 FR_hip_joint_force=500;     FR_thigh_joint_force=500;         FR_calf_joint_force=500; %第二条腿
 RL_hip_joint_force=500;     RL_thigh_joint_force=500;         RL_calf_joint_force=500; %第三条腿   
 FL_hip_joint_force=500;     FL_thigh_joint_force=500;         FL_calf_joint_force=500; %第四条腿
 %12个电机角度参数 弧度值 
 RL_hip_joint_pos=0*pi/180;   RL_thigh_joint_pos=0*pi/180;   RL_calf_joint_pos=0*pi/180;                     
 FL_hip_joint_pos=0*pi/180;   FL_thigh_joint_pos=0*pi/180;   FL_calf_joint_pos=0*pi/180;   
 RR_hip_joint_pos=0*pi/180;   RR_thigh_joint_pos=0*pi/180;   RR_calf_joint_pos=0*pi/180;   
 FR_hip_joint_pos=0*pi/180;   FR_thigh_joint_pos=0*pi/180;   FR_calf_joint_pos=0*pi/180;   
 
%   RL_hip_joint_pos=30*pi/180;   RL_thigh_joint_pos=30*pi/180;   RL_calf_joint_pos=70*pi/180;                     
%  FL_hip_joint_pos=30*pi/180;   FL_thigh_joint_pos=30*pi/180;   FL_calf_joint_pos=70*pi/180;   
%  RR_hip_joint_pos=30*pi/180;   RR_thigh_joint_pos=30*pi/180;   RR_calf_joint_pos=70*pi/180;   
%  FR_hip_joint_pos=30*pi/180;   FR_thigh_joint_pos=30*pi/180;   FR_calf_joint_pos=70*pi/180;   
 %设置电机力矩
 rec=vrep.simxSetJointForce(clientID,RR_calf_joint, RR_calf_joint_force,vrep.simx_opmode_blocking);
 rec=vrep.simxSetJointForce(clientID,FR_calf_joint, FR_calf_joint_force,vrep.simx_opmode_blocking);
 rec=vrep.simxSetJointForce(clientID,RR_thigh_joint, RR_thigh_joint_force,vrep.simx_opmode_blocking);
 rec=vrep.simxSetJointForce(clientID,FR_thigh_joint, FR_thigh_joint_force,vrep.simx_opmode_blocking);
 rec=vrep.simxSetJointForce(clientID,RR_hip_joint, RR_hip_joint_force,vrep.simx_opmode_blocking);
 rec=vrep.simxSetJointForce(clientID,FR_hip_joint, FR_hip_joint_force,vrep.simx_opmode_blocking);
 rec=vrep.simxSetJointForce(clientID,RL_calf_joint, RL_calf_joint_force,vrep.simx_opmode_blocking);
 rec=vrep.simxSetJointForce(clientID,FL_calf_joint, FL_calf_joint_force,vrep.simx_opmode_blocking);
 rec=vrep.simxSetJointForce(clientID,RL_thigh_joint, RL_thigh_joint_force,vrep.simx_opmode_blocking);
 rec=vrep.simxSetJointForce(clientID,FL_thigh_joint, FL_thigh_joint_force,vrep.simx_opmode_blocking);
 rec=vrep.simxSetJointForce(clientID,RL_hip_joint, RL_hip_joint_force,vrep.simx_opmode_blocking);
 rec=vrep.simxSetJointForce(clientID,FL_hip_joint, FL_hip_joint_force,vrep.simx_opmode_blocking);
 
 pause(1);   %延时1s
 t=clock;   %获取matlab系统当前时间 
 startTime=t(5)*60+t(6); %当前时间 [年 月 日 时 分 秒]
 currentTime=0; %当前时间
 gait_state=2;  %步态标志位

% pos test
% row=0;    pitch=0;    yaw=0;    
% pos_x=0.05;     pos_y=0.05;     pos_z=-0.1;%z为负下降

%raw text
% row=0;    pitch=0;    yaw=15;    
% pos_x=0;     pos_y=0;     pos_z=-0.1;%z为负下降

%raw text
% row=0;    pitch=10;    yaw=0;    
% pos_x=0;     pos_y=0;     pos_z=-0.1;%z为负下降

%raw text
row=8;    pitch=0;    yaw=0;    
pos_x=0;     pos_y=0;     pos_z=-0.1;%z为负下降
while(1)
 [lb_x,lb_y,lb_z,rb_x,rb_y,rb_z,rf_x,rf_y,rf_z,lf_x,lf_y,lf_z] = pose_control(row,pitch,yaw,pos_x,pos_y,pos_z);
 [FL_hip_joint_pos,FL_thigh_joint_pos,FL_calf_joint_pos]=xyz(lf_x,lf_y,lf_z);
 [RL_hip_joint_pos,RL_thigh_joint_pos,RL_calf_joint_pos]=xyz(lb_x,lb_y,lb_z);
 [FR_hip_joint_pos,FR_thigh_joint_pos,FR_calf_joint_pos]=xyz(rf_x,rf_y,rf_z);
 [RR_hip_joint_pos,RR_thigh_joint_pos,RR_calf_joint_pos]=xyz(rb_x,rb_y,rb_z);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%逆运动学
%x正前负后 y外负内正 z=-0.42伸直 
% [FL_hip_joint_pos,FL_thigh_joint_pos,FL_calf_joint_pos]=xyz(0,-0.0851,-0.42);
% [RL_hip_joint_pos,RL_thigh_joint_pos,RL_calf_joint_pos]=xyz(0,-0.0851,-0.42); %x正前负后 y外负内正 z=-0.42伸直 
% 
% [FR_hip_joint_pos,FR_thigh_joint_pos,FR_calf_joint_pos]=xyz(0,-0.0851,-0.42);
% [RR_hip_joint_pos,RR_thigh_joint_pos,RR_calf_joint_pos]=xyz(0,-0.0851,-0.42); %x正前负后 y外正内负 z=-0.42伸直 

%修改z
% [FL_hip_joint_pos,FL_thigh_joint_pos,FL_calf_joint_pos]=xyz(0,-0.0851,-0.42);
% [RL_hip_joint_pos,RL_thigh_joint_pos,RL_calf_joint_pos]=xyz(0,-0.0851,-0.35); %x正前负后 y外负内正 z=-0.42伸直 
% 
% [FR_hip_joint_pos,FR_thigh_joint_pos,FR_calf_joint_pos]=xyz(0,-0.0851,-0.3);
% [RR_hip_joint_pos,RR_thigh_joint_pos,RR_calf_joint_pos]=xyz(0,-0.0851,-0.25); %x正前负后 y外正内负 z=-0.42伸直 

% [FL_hip_joint_pos,FL_thigh_joint_pos,FL_calf_joint_pos]=xyz(0.1,-0.0851,-0.3);
% [RL_hip_joint_pos,RL_thigh_joint_pos,RL_calf_joint_pos]=xyz(0,-0.0851,-0.3); %x正前负后 y外负内正 z=-0.42伸直 
% 
% [FR_hip_joint_pos,FR_thigh_joint_pos,FR_calf_joint_pos]=xyz(-0.1,-0.0851,-0.3);
% [RR_hip_joint_pos,RR_thigh_joint_pos,RR_calf_joint_pos]=xyz(-0.2,-0.0851,-0.3); %x正前负后 y外正内负 z=-0.42伸直 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%逆运动学


 %电机控制函数
 %由于模型导入原因 calf关节初始角度-52.5且只有输入角度小于-52.5才有效
 rec=vrep.simxSetJointTargetPosition(clientID,FL_hip_joint,-FL_hip_joint_pos,vrep.simx_opmode_oneshot);
 rec=vrep.simxSetJointTargetPosition(clientID,FL_thigh_joint,FL_thigh_joint_pos,vrep.simx_opmode_oneshot);
 rec=vrep.simxSetJointTargetPosition(clientID,FL_calf_joint,FL_calf_joint_pos-52.5*pi/180,vrep.simx_opmode_oneshot); 

 rec=vrep.simxSetJointTargetPosition(clientID,RL_hip_joint,-RL_hip_joint_pos,vrep.simx_opmode_oneshot);
 rec=vrep.simxSetJointTargetPosition(clientID,RL_thigh_joint,RL_thigh_joint_pos,vrep.simx_opmode_oneshot);
 rec=vrep.simxSetJointTargetPosition(clientID,RL_calf_joint,RL_calf_joint_pos-52.5*pi/180,vrep.simx_opmode_oneshot);
 
 rec=vrep.simxSetJointTargetPosition(clientID,FR_hip_joint,FR_hip_joint_pos,vrep.simx_opmode_oneshot);
 rec=vrep.simxSetJointTargetPosition(clientID,FR_thigh_joint,FR_thigh_joint_pos,vrep.simx_opmode_oneshot);
 rec=vrep.simxSetJointTargetPosition(clientID,FR_calf_joint,FR_calf_joint_pos-52.5*pi/180,vrep.simx_opmode_oneshot);
 
 rec=vrep.simxSetJointTargetPosition(clientID,RR_hip_joint,RR_hip_joint_pos,vrep.simx_opmode_oneshot);
 rec=vrep.simxSetJointTargetPosition(clientID,RR_thigh_joint,RR_thigh_joint_pos,vrep.simx_opmode_oneshot);
 rec=vrep.simxSetJointTargetPosition(clientID,RR_calf_joint,RR_calf_joint_pos-52.5*pi/180,vrep.simx_opmode_oneshot);

end
        vrep.simxStopSimulation(clientID,vrep.simx_opmode_blocking); %仿真停止
        vrep.simxFinish(clientID);
else
        disp('Failed connecting to remote API server');
end
    vrep.delete(); 
    disp('Program ended');

解算函数

function [rb_x,rb_y,rb_z,rf_x,rf_y,rf_z,lb_x,lb_y,lb_z,lf_x,lf_y,lf_z] = pose_control(row,pitch,yaw,pos_x,pos_y,pos_z)
b=0.094;
l=0.366;
w=0.2641;
h=0.42;
%转为弧度制
R=row*pi/180   ;
P=pitch*pi/180 ;
Y=yaw*pi/180   ;
pos=[pos_x,pos_y,pos_z]';
%旋转变化矩阵
rotx=([[ 1,        0,         0      ]
       [ 0,        cos(R),   -sin(R) ]
       [ 0,        sin(R),    cos(R) ]]);
roty=([[ cos(P),   0,        -sin(P) ]
       [ 0,        1,         0      ]
       [ sin(P),   0,         cos(P) ]]);
rotz=([[ cos(Y),  -sin(Y),    0      ]
       [ sin(Y),   cos(Y),    0      ]
       [ 0,        0,         1      ]]);
rot_mat = rotx * roty * rotz;
%结构参数 躯干四个点相对地面位置信息
  body_struct = ([[ l / 2,  -b / 2,  h]     
                 [  l / 2, b / 2,   h]     
                 [ -l / 2,  b / 2,    h]     
                 [ -l / 2, -b / 2,   h]])'; 
%结构参数 四个足端点相对地面位置信息             
 footpoint_struct = ([[ l/2,   -w/2,  0]
                     [  l/2,   w/2,  0]
                     [  -l/2,    w/2,  0]
                     [  -l/2,    -w/2,  0]])';
leg_pose = zeros(3, 4);
for i= 1:4
    leg_pose(:,i) =  pos + rot_mat * body_struct(:, i) - footpoint_struct(:, i);
end

 rb_x = (leg_pose(1, 1));
 rb_y = -(leg_pose(2, 1));
 rb_z = -(leg_pose(3, 1));
 rf_x = (leg_pose(1, 2));
 rf_y = (leg_pose(2, 2));
 rf_z = -(leg_pose(3, 2));
 lb_x = (leg_pose(1, 3));
 lb_y = (leg_pose(2, 3));
 lb_z = -(leg_pose(3, 3));
 lf_x = (leg_pose(1, 4));
 lf_y = -(leg_pose(2, 4));
 lf_z = -(leg_pose(3, 4));
end

代码测试

case1 pos测试

row=0;    pitch=0;    yaw=0;    
pos_x=0.05;     pos_y=0.05;     pos_z=-0.1;%z为负下降

在这里插入图片描述

yaw测试

row=0;    pitch=0;    yaw=15;    
pos_x=0;     pos_y=0;     pos_z=-0.1;%z为负下降

在这里插入图片描述

pitch 测试

row=0;    pitch=10;    yaw=0;    
pos_x=0;     pos_y=0;     pos_z=-0.1;%z为负下降

在这里插入图片描述

raw 测试

row=8;    pitch=0;    yaw=0;    
pos_x=0;     pos_y=0;     pos_z=-0.1;%z为负下降

在这里插入图片描述

  • 0
    点赞
  • 7
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值