结构参数获取
参数解释
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为负下降