平面二连杆逆运动学仿真Vrep_matlab

                       平面二连杆逆运动学仿真  Vrep_matlab

1.创建平面二连杆模型

  1.   打开vrep,新建scene。

  2. 点击Add->Dummy,调整Dummy大小、颜色、位置。命名为Base.

     

  3. 点击Add->Joint->Revolute,命名为j1。双击j1,在scene object properties中将Mode设置为Inverse kinematics mode。length设置为0.15、Diameter设置为0.04。

  4. 点击Add->Primitive shape->Cylinder,设置参数X-size为0.05m,Z-size为0.5m。并调整颜色、位置。如下图。

     

     

  5. 按照上述创建关节与连杆的方式,再创建关节二,参数设置与上述相同。

  6. 再按下图调整各部件的关系图。

  7. 添加dummy对,点击Add添加dummy,命名为tip,并调整其大小、位置。再添加target,同样设置其参数。最后调整层次结构。配对dummy对,如图(其实可以不添加)

     

     

    至此,模型创建完成。 此处模型购买链接,提供咨询服务。

2.推导平面逆运动学方程

由余弦定理得

                                              

式-1

 

又由三角学知识可得

式2

 

代入上式求得

式3

时,上式有解。

式4

求解关节角 θ1,得

式5

再次应用余弦定理得

式6

 

其中β∈(0,π)

此处模型购买链接,提供咨询服务。 

   3.Matlab代码实现

在matlab中新建函数,函数名为InverseKinematics,添加如下代码。

function [theta1,theta2] = InverseKinematics(Px,Py,a1,a2)
    %   此处显示详细说明
    
    if Px^2+Py^2<=(a1+a2)^2
        
       %关节2角度
       theta2=acos((Px^2+Py^2-a1^2-a2^2)/(2*a1*a2));
       
       %关节1角度
       if (Px<0 && Py>0)
            alaph=-pi+atan(Py/Px);
       elseif (Px<0 && Py<0)
           alaph=pi+atan(Py/Px);
       else
           alaph=atan(Py/Px);
       end
       beta=acos((Px^2+Py^2+a1^2-a2^2)/(2*a1*sqrt(Px^2+Py^2)));
       if  theta2>=0
           
            theta1=alaph-beta;
       else
            theta1=alaph+beta;
       end
       fprintf("alaph: %f°  , beta:  %f  \n",alaph*180/pi,beta*180/pi);

        fprintf("theta1:  %f°, theta2:  %f°\n",theta1*180/pi,theta2*180/pi);
    else
        
        disp('目标点不在工作范围内!');
        
    end

    
end

添加新脚本

%两连杆机械臂控制脚本

%建立matlab与vrep通信通道
    disp('Program started');
    % sim=remApi('remoteApi','extApi.h'); % using the header (requires a compiler)
    sim=remApi('remoteApi'); % using the prototype file (remoteApiProto.m)
    sim.simxFinish(-1); % just in case, close all opened connections
    clientID=sim.simxStart('127.0.0.1',19999,true,true,5000,5);
    
%获取vrep模型参数
     if (clientID>-1)
                disp('Connected to remote API server!!!!');
     end
     a1=0.5;
     a2=0.5;
     %Position=[0,0];
     %取得目标点句柄                                           
     [returnCode,P_handle]=sim.simxGetObjectHandle(clientID,'target',sim.simx_opmode_oneshot_wait);
     %基坐标系句柄
     [returnCode,Base_handle]=sim.simxGetObjectHandle(clientID,'base',sim.simx_opmode_oneshot_wait);
     %取得目标点坐标
     [returnCode,Position]=sim.simxGetObjectPosition(clientID,P_handle,Base_handle,sim.simx_opmode_oneshot_wait);
     Px=Position(1);
     Py=Position(2);
     fprintf("Px: %f  ,Py:  %f \n",Px,Py);
%逆运动学解算
    [theta1,theta2]=InverseKinematics(Px,Py,a1,a2);
%关节角发送至vrep
%关节句柄
 [returnCode,J1_handle]=sim.simxGetObjectHandle(clientID,'j1',sim.simx_opmode_oneshot_wait);
 [returnCode,J2_handle]=sim.simxGetObjectHandle(clientID,'j2',sim.simx_opmode_oneshot_wait);
 %设置关节角度
 sim.simxSetJointPosition(clientID,J1_handle,theta1,sim.simx_opmode_oneshot);
 sim.simxSetJointPosition(clientID,J2_handle,theta2,sim.simx_opmode_oneshot);

 
 

 

视频观看地址 https://www.bilibili.com/video/bv1Xg4y1v7pR

vrep Q群 609286340

此处模型购买链接,提供咨询服务。

vrep_matlab平面二连杆逆运动学仿真

 

posted @ 2020-07-13 22:32  秋风不识春  阅读(1442)  评论(0编辑  收藏  举报