%sizu_test
%通讯初始化
clear
clc
disp('Program started');
% sim=remApi('remoteApi','extApi.h'); % using the header (requires a compiler)
vrep1=remApi('remoteApi'); % using the prototype file (remoteApiProto.m)
vrep1.simxFinish(-1); % just in case, close all opened connections
clientID=vrep1.simxStart('127.0.0.1',18888,true,true,5000,5);%返回值为0通讯成功,否则-1
disp(clientID);
if(clientID>-1)
disp("Connected to remote API servver");
vrep1.simxStartSimulation(clientID,vrep1.simx_opmode_oneshot);%开启仿真
%声明节点,关节初始化,12个关节
[rec ,rb_rot_3]=vrep1.simxGetObjectHandle (clientID,'rb_rot_3',vrep1.simx_opmode_blocking);
[rec ,rf_rot_3]=vrep1.simxGetObjectHandle (clientID,'rf_rot_3',vrep1.simx_opmode_blocking);
[rec ,rb_rot_2]=vrep1.simxGetObjectHandle (clientID,'rb_rot_2',vrep1.simx_opmode_blocking);
[rec ,rf_rot_2]=vrep1.simxGetObjectHandle (clientID,'rf_rot_2',vrep1.simx_opmode_blocking);
[rec ,rb_rot_1]=vrep1.simxGetObjectHandle (clientID,'rb_rot_1',vrep1.simx_opmode_blocking);
[rec ,rf_rot_1]=vrep1.simxGetObjectHandle (clientID,'rf_rot_1',vrep1.simx_opmode_blocking);
[rec ,lb_rot_3]=vrep1.simxGetObjectHandle (clientID,'lb_rot_3',vrep1.simx_opmode_blocking);
[rec ,lf_rot_3]=vrep1.simxGetObjectHandle (clientID,'lf_rot_3',vrep1.simx_opmode_blocking);
[rec ,lb_rot_2]=vrep1.simxGetObjectHandle (clientID,'lb_rot_2',vrep1.simx_opmode_blocking);
[rec ,lf_rot_2]=vrep1.simxGetObjectHandle (clientID,'lf_rot_2',vrep1.simx_opmode_blocking);
[rec ,lb_rot_1]=vrep1.simxGetObjectHandle (clientID,'lb_rot_1',vrep1.simx_opmode_blocking);
[rec ,lf_rot_1]=vrep1.simxGetObjectHandle (clientID,'lf_rot_1',vrep1.simx_opmode_blocking);
%12个电机力矩参数
rb_rot_1_force=500; rb_rot_2_force=500; rb_rot_3_force=500; %第一条腿
rf_rot_1_force=500; rf_rot_2_force=500; rf_rot_3_force=500; %第二条腿
lb_rot_1_force=500; lb_rot_2_force=500; lb_rot_3_force=500; %第三条腿
lf_rot_1_force=500; lf_rot_2_force=500; lf_rot_3_force=500; %第四条腿
%12个电机角度参数
rb_rot_1_pos=0; rb_rot_2_pos=0; rb_rot_3_pos=0; %第一条腿
rf_rot_1_pos=0; rf_rot_2_pos=0; rf_rot_3_pos=0; %第二条腿
lb_rot_1_pos=0; lb_rot_2_pos=0; lb_rot_3_pos=0; %第三条腿
lf_rot_1_pos=0; lf_rot_2_pos=0; lf_rot_3_pos=0; %第四条腿
%设置电机力矩
rec=vrep1.simxSetJointForce(clientID,rb_rot_3, rb_rot_3_force,vrep1.simx_opmode_blocking);
rec=vrep1.simxSetJointForce(clientID,rf_rot_3, rf_rot_3_force,vrep1.simx_opmode_blocking);
rec=vrep1.simxSetJointForce(clientID,rb_rot_2, rb_rot_2_force,vrep1.simx_opmode_blocking);
rec=vrep1.simxSetJointForce(clientID,rf_rot_2, rf_rot_2_force,vrep1.simx_opmode_blocking);
rec=vrep1.simxSetJointForce(clientID,rb_rot_1, rb_rot_1_force,vrep1.simx_opmode_blocking);
rec=vrep1.simxSetJointForce(clientID,rf_rot_1, rf_rot_1_force,vrep1.simx_opmode_blocking);
rec=vrep1.simxSetJointForce(clientID,lb_rot_3, lb_rot_3_force,vrep1.simx_opmode_blocking);
rec=vrep1.simxSetJointForce(clientID,lf_rot_3, lf_rot_3_force,vrep1.simx_opmode_blocking);
rec=vrep1.simxSetJointForce(clientID,lb_rot_2, lb_rot_2_force,vrep1.simx_opmode_blocking);
rec=vrep1.simxSetJointForce(clientID,lf_rot_2, lf_rot_2_force,vrep1.simx_opmode_blocking);
rec=vrep1.simxSetJointForce(clientID,lb_rot_1, lb_rot_1_force,vrep1.simx_opmode_blocking);
rec=vrep1.simxSetJointForce(clientID,lf_rot_1, lf_rot_1_force,vrep1.simx_opmode_blocking);
row=0; pitch=0; yaw=0;
pos_x=0; pos_y=0.2; pos_z=0;
pause(1); %延时1s
while 1
%matlab当前时间,从进入主循环开始
[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);
[lb_rot_1_pos,lb_rot_2_pos,lb_rot_3_pos]=xyz(lb_x,lb_y,lb_z);
[lf_rot_1_pos,lf_rot_2_pos,lf_rot_3_pos]=xyz(lf_x,lf_y,lf_z);
[rb_rot_1_pos,rb_rot_2_pos,rb_rot_3_pos]=xyz(rb_x,rb_y,rb_z);
[rf_rot_1_pos,rf_rot_2_pos,rf_rot_3_pos]=xyz(rf_x,rf_y,rf_z);
%单腿逆运动学,Y值设定为默认值
%[lb_rot_1_pos,lb_rot_2_pos,lb_rot_3_pos]=xyz(lb_x,-0.15,lb_z);
%[lf_rot_1_pos,lf_rot_2_pos,lf_rot_3_pos]=xyz(lf_x,-0.15,lf_z);
%[rb_rot_1_pos,rb_rot_2_pos,rb_rot_3_pos]=xyz(rb_x,-0.15,rb_z);
%[rf_rot_1_pos,rf_rot_2_pos,rf_rot_3_pos]=xyz(rf_x,-0.15,rf_z);
%电机控制函数
rec=vrep1.simxSetJointTargetPosition(clientID,lb_rot_1,-lb_rot_1_pos,vrep1.simx_opmode_oneshot);
rec=vrep1.simxSetJointTargetPosition(clientID,lb_rot_2,lb_rot_2_pos,vrep1.simx_opmode_oneshot);
rec=vrep1.simxSetJointTargetPosition(clientID,lb_rot_3,lb_rot_3_pos,vrep1.simx_opmode_oneshot);
rec=vrep1.simxSetJointTargetPosition(clientID,rf_rot_1,rf_rot_1_pos,vrep1.simx_opmode_oneshot);
rec=vrep1.simxSetJointTargetPosition(clientID,rf_rot_2,rf_rot_2_pos,vrep1.simx_opmode_oneshot);
rec=vrep1.simxSetJointTargetPosition(clientID,rf_rot_3,rf_rot_3_pos,vrep1.simx_opmode_oneshot);
rec=vrep1.simxSetJointTargetPosition(clientID,rb_rot_1,-rb_rot_1_pos,vrep1.simx_opmode_oneshot);
rec=vrep1.simxSetJointTargetPosition(clientID,rb_rot_2,rb_rot_2_pos,vrep1.simx_opmode_oneshot);
rec=vrep1.simxSetJointTargetPosition(clientID,rb_rot_3,rb_rot_3_pos,vrep1.simx_opmode_oneshot);
rec=vrep1.simxSetJointTargetPosition(clientID,lf_rot_1,lf_rot_1_pos,vrep1.simx_opmode_oneshot);
rec=vrep1.simxSetJointTargetPosition(clientID,lf_rot_2,lf_rot_2_pos,vrep1.simx_opmode_oneshot);
rec=vrep1.simxSetJointTargetPosition(clientID,lf_rot_3,lf_rot_3_pos,vrep1.simx_opmode_oneshot);
end
vrep1.simxStopSimulation(clientID,vrep1.simx_opmode_blocking); %仿真停止
vrep1.simxFinish(clientID);
else
disp('Failed connecting to remote API server');
end
vrep1.delete();
disp('Program ended');
展示全文
第三方账号登入
QQ 微博 微信