Matlab与V-rep联合仿真 逆运动学
Matlab联合仿真环境搭建
复制api文件到新建文件夹
路径：
C:\Program Files\CoppeliaRobotics\CoppeliaSimEdu\programming\remoteApiBindings\matlab\matlab
matlab找到对应目录
v-rep配置
添加内嵌脚本
选择对象
双击出现代码框,并添加语句，启动远程控制功能
代码测试
1.在v-rep中点击运行
2.MATLAB运行，simpleTest
版本不匹配
测试过程中出现“Note: always make sure you use the corresponding remoteApi library
(i.e. 32bit Matlab will not work with 64bit remoteApi, and vice-versa)”
需要将下路径中的remoteApi.dll文件复制到MATLAB代码文件夹中
C:\Program Files\CoppeliaRobotics\CoppeliaSimEdu\programming\remoteApiBindings\lib\lib\Windows
问题解决
控制代码测试
可以通过手册查找函数用法
逆运动学解算
杆件长度参数确定
同时选中两个坐标系，观察坐标系间距离结课获取相关参数。
此处应当重点注意，由于选择两坐标系所得距离并非calf_joint与地面间距离，所以不可直接使用，否则腿部无法伸直。
在SolidWorks中打开calf.stl,测量可得L3实际长度为0.22m，给响应坐标腿顺利伸直。
所以L1=0.0851,L2=0.2,L3=0.22
MATLAB代码
逆运动学解算
function [theta_1,theta_2,theta_3] = xyz(x,y,z)
%与课程对应
y=-y;
z=-z;
%
L1=0.0851;
L2=0.2;
L3=0.22; %腿长不是0.2
dyz=sqrt(y.^2+z.^2);
lyz=sqrt(dyz.^2-L1.^2);
theta_1_yz=-atan(y/z);
theta_1_h_offset=-atan(L1./lyz);
theta_1=theta_1_yz-theta_1_h_offset;
%
lxzp=sqrt(lyz.^2+x.^2);
n=(lxzp.^2-L3.^2-L2.^2)/(2*L2);
theta_3=-acos(n/L3);
%
theta_2_xzp=-atan(x/lyz);
theta_2_off=acos((L2+n)/lxzp);
theta_2=theta_2_xzp+theta_2_off;
%输出角度为弧度
end
联合仿真.m
%通讯初始化 从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);
% row=0; pitch=0; yaw=0;
% pos_x=0; pos_y=0.2; pos_z=-0.2;
pause(1); %延时1s
t=clock; %获取matlab系统当前时间
startTime=t(5)*60+t(6); %当前时间 [年 月 日 时 分 秒]
currentTime=0; %当前时间
gait_state=2; %步态标志位
% [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);
%
% RL_calf_joint_pos=-52.5*pi/180-RL_calf_joint_pos;
% FL_calf_joint_pos=-52.5*pi/180-RL_calf_joint_pos;
% RR_calf_joint_pos=-52.5*pi/180-RL_calf_joint_pos;
% FR_calf_joint_pos=-52.5*pi/180-RL_calf_joint_pos;
while(1)
%x正前负后 y外负内正 z=-0.42伸直
[FL_hip_joint_pos,FL_thigh_joint_pos,FL_calf_joint_pos]=xyz(0,-0.0851,-0.35);
[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.35);
[RR_hip_joint_pos,RR_thigh_joint_pos,RR_calf_joint_pos]=xyz(0,-0.0851,-0.35); %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');
四腿完全伸直
[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伸直
四个足端相同xy坐标不同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伸直
四个足端相同yz坐标不同x坐标得到如下测试结果
[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伸直
四个足端相同xz坐标不同y坐标得到如下测试结果
[FL_hip_joint_pos,FL_thigh_joint_pos,FL_calf_joint_pos]=xyz(0,-0.12,-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,-0.04,-0.3);
[RR_hip_joint_pos,RR_thigh_joint_pos,RR_calf_joint_pos]=xyz(0,0,-0.3); %x正前负后 y外正内负 z=-0.42伸直
