[Python知识库]【Coppeliasim】 matlab /python 与Coppeliasim通信

?

matlab源码

matlab主程序：

clear

%% 初始化
systemInitialization;

% youBot的尺码信息
wheel_R = 0.05;
a = 0.165;
b = 0.228;

% 用户变量
simu_time = 0;
center_velocity = [0,0,0];
desired_wheel_velocities = [0,0,0,0];

%% 主控制循环
disp('begin main control loop ...');
while true
    % 运动规划
    simu_time = simu_time + 0.05;
    
    if simu_time < 1
        center_velocity = [0, 0.1, 0];
    elseif simu_time < 2
        center_velocity = [0, -0.1, 0];
    elseif simu_time < 3
        center_velocity = [0.1, 0, 0];
    elseif simu_time < 4
        center_velocity = [-0.1, 0, 0];
    elseif simu_time < 5
        center_velocity = [0.1, 0.1, 0];
    elseif simu_time < 6
        center_velocity = [-0.1, -0.1, 0];
    elseif simu_time < 7
        center_velocity = [0, 0, pi/10];
    elseif simu_time < 8
        center_velocity = [0, 0, -pi/10];
    elseif simu_time < 9
        center_velocity = [0, 0, -pi/10];
    elseif simu_time < 10
        center_velocity = [0, 0, pi/10];
    elseif simu_time < 11
        center_velocity = [0, 0, 0];
    else
        break;
    end

    % 逆运动学计算
    desired_wheel_velocities = chassisInverseKinematics(center_velocity(1), center_velocity(2), center_velocity(3), wheel_R, a, b);

    % 控制youBot机器人
    % VREP_armControl(vrep_sim, clientID, arm_joints_handle, desired_arm_joint_angles);
    VREP_wheelsControl(vrep_sim, clientID, wheel_joints_handle, desired_wheel_velocities);
    pause(0.001);
end

% 现在以非阻塞方式向 CoppeliaSim 发送一些数据：
vrep_sim.simxAddStatusbarMessage(clientID,'Over!',vrep_sim.simx_opmode_oneshot);

% 在关闭与 CoppeliaSim 的连接之前，请确保发出的最后一个命令有时间到达。您可以通过（例如）保证这一点：
vrep_sim.simxGetPingTime(clientID);

% 现在关闭与 CoppeliaSim 的连接：
vrep_sim.simxFinish(clientID);

% 调用析构函数
vrep_sim.delete();
disp('Program ended');

初始化：

%% 系统初始化
disp('System Initialization');

vrep_sim=remApi('remoteApi');
vrep_sim.simxFinish(-1); % just in case, close all opened connections
clientID=vrep_sim.simxStart('127.0.0.1',19999,true,true,5000,5);

if (clientID>-1)
    disp('Connected to remote API server');

    [return_code, youBot_handle] = vrep_sim.simxGetObjectHandle(clientID, 'youBot', vrep_sim.simx_opmode_blocking);
    if (return_code == vrep_sim.simx_return_ok)
        disp('get object youBot ok.');
    end

    [return_code, youBot_dummy_handle] = vrep_sim.simxGetObjectHandle(clientID, 'youBotDummy', vrep_sim.simx_opmode_blocking);
    if (return_code == vrep_sim.simx_return_ok)
        disp('get object youBotDummy ok.');
    end

    % Prepare initial values for four wheels
    wheel_joints_handle = [-1,-1,-1,-1]; % front left, rear left, rear right, front right
    [return_code, wheel_joints_handle(1)] = vrep_sim.simxGetObjectHandle(clientID, 'rollingJoint_fr', vrep_sim.simx_opmode_blocking);
    if (return_code == vrep_sim.simx_return_ok)
        disp('get object youBot rollingJoint_fl ok.');
    end

    [return_code, wheel_joints_handle(2)] = vrep_sim.simxGetObjectHandle(clientID, 'rollingJoint_fl', vrep_sim.simx_opmode_blocking);
    if (return_code == vrep_sim.simx_return_ok)
        disp('get object youBot rollingJoint_rl ok.');
    end

    [return_code, wheel_joints_handle(3)] = vrep_sim.simxGetObjectHandle(clientID, 'rollingJoint_rl', vrep_sim.simx_opmode_blocking);
    if (return_code == vrep_sim.simx_return_ok)
        disp('get object youBot rollingJoint_rr ok.');
    end

    [return_code, wheel_joints_handle(4)] = vrep_sim.simxGetObjectHandle(clientID, 'rollingJoint_rr', vrep_sim.simx_opmode_blocking);
    if (return_code == vrep_sim.simx_return_ok)
        disp('get object youBot rollingJoint_fr ok.');
    end
    
    % Prepare initial values for five arm joints
    arm_joints_handle = [-1,-1,-1,-1,-1];
    for i=0:4
        [return_code, arm_joints_handle(i+1)] = vrep_sim.simxGetObjectHandle(clientID, strcat('youBotArmJoint', num2str(i)), vrep_sim.simx_opmode_blocking);
        if (return_code == vrep_sim.simx_return_ok)
            disp(strcat('get object arm joint ', num2str(i), ' ok.'));
        end
    end
    
    % Desired joint positions for initialization
    desired_arm_joint_angles = [180*pi/180, 30.91*pi/180, 52.42*pi/180, 72.68*pi/180, 0];
    
    % Initialization all arm joints
    for i = 1:5
        vrep_sim.simxSetJointPosition(clientID, arm_joints_handle(i), desired_arm_joint_angles(i), vrep_sim.simx_opmode_blocking);
    end
else
    disp('Failed connecting to remote API server');
    pause();
end

逆运动学

function [v_wheel] = chassisInverseKinematics(vx, vy, omega, wheel_R, a, b)
    omega_1 = (vy - vx + (a+b)*omega)/wheel_R;
    omega_2 = (vy + vx - (a+b)*omega)/wheel_R;
    omega_3 = (vy - vx - (a+b)*omega)/wheel_R;
    omega_4 = (vy + vx + (a+b)*omega)/wheel_R;
    
    % set the direction for each wheel
    v_wheel = [0,0,0,0];
    v_wheel(1) = -omega_1;
    v_wheel(2) = -omega_2;
    v_wheel(3) = -omega_3;
    v_wheel(4) = -omega_4;
end

机械臂控制：

function VREP_armControl(vrep_sim, clientID, arm_joints_handle, desired_arm_joint_angles)
    for i = 1:5
        vrep_sim.simxSetJointPosition(clientID, arm_joints_handle(i), desired_arm_joint_angles(i), vrep_sim.simx_opmode_blocking);
    end
end

轮子控制：

function VREP_wheelsControl(vrep_sim, clientID, wheel_joints_handle, desired_wheel_velocities)
    for i = 1:4
      vrep_sim.simxSetJointTargetVelocity(clientID, wheel_joints_handle(i), desired_wheel_velocities(i), vrep_sim.simx_opmode_blocking);
  end
end

Python源码

import time
import math
import sys
sys.path.append('./VREP_remoteAPIs')

''' 反向运动学'''
def chassisInverseKinematics(vx, vy, omega, wheel_R, a, b):
    omega_1 = (vy - vx + (a+b)*omega)/wheel_R
    omega_2 = (vy + vx - (a+b)*omega)/wheel_R
    omega_3 = (vy - vx - (a+b)*omega)/wheel_R
    omega_4 = (vy + vx + (a+b)*omega)/wheel_R
    
    # set the direction for each wheel
    v_wheel = [0,0,0,0]
    v_wheel[0] = -omega_1
    v_wheel[1] = -omega_2
    v_wheel[2] = -omega_3
    v_wheel[3] = -omega_4

    return v_wheel

''' youBot的臂控功能 '''
def VREP_armControl(vrep_sim, clientID, arm_joints_handle, desired_arm_joint_angles):
    for i in range(0,5):
        vrep_sim.simxSetJointPosition(clientID, arm_joints_handle[i], desired_arm_joint_angles[i], vrep_sim.simx_opmode_blocking)

''' youBot的轮子控制功能 '''
def VREP_wheelsControl(vrep_sim, clientID, wheel_joints_handle, desired_wheel_velocities):
    for i in range(0,4):
        vrep_sim.simxSetJointTargetVelocity(clientID, wheel_joints_handle[i], desired_wheel_velocities[i], vrep_sim.simx_opmode_blocking)


if __name__ == '__main__':
    try:
        import sim as vrep_sim
    except:
        print ('--------------------------------------------------------------')
        print ('"sim.py" could not be imported. This means very probably that')
        print ('either "sim.py" or the remoteApi library could not be found.')
        print ('Make sure both are in the same folder as this file,')
        print ('or appropriately adjust the file "sim.py"')
        print ('--------------------------------------------------------------')
        print ('')

    ''' 初始化 '''
    print ('Program started')
    vrep_sim.simxFinish(-1) # just in case, close all opened connections
    clientID = vrep_sim.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to CoppeliaSim
    if clientID != -1:
        print ('Connected to remote API server')

        return_code, youBot_handle = vrep_sim.simxGetObjectHandle(clientID, 'youBot', vrep_sim.simx_opmode_blocking)
        if (return_code == vrep_sim.simx_return_ok):
            print('get object youBot ok.')

        return_code, youBot_dummy_handle = vrep_sim.simxGetObjectHandle(clientID, 'youBotDummy', vrep_sim.simx_opmode_blocking)
        if (return_code == vrep_sim.simx_return_ok):
            print('get object youBotDummy ok.')

        # 准备四个轮子的初始值
        wheel_joints_handle = [-1,-1,-1,-1]; # front left, rear left, rear right, front right
        return_code, wheel_joints_handle[0] = vrep_sim.simxGetObjectHandle(clientID, 'rollingJoint_fr', vrep_sim.simx_opmode_blocking)
        if (return_code == vrep_sim.simx_return_ok):
            print('get object youBot rollingJoint_fl ok.')

        return_code, wheel_joints_handle[1] = vrep_sim.simxGetObjectHandle(clientID, 'rollingJoint_fl', vrep_sim.simx_opmode_blocking)
        if (return_code == vrep_sim.simx_return_ok):
            print('get object youBot rollingJoint_rl ok.')

        return_code, wheel_joints_handle[2] = vrep_sim.simxGetObjectHandle(clientID, 'rollingJoint_rl', vrep_sim.simx_opmode_blocking)
        if (return_code == vrep_sim.simx_return_ok):
            print('get object youBot rollingJoint_rr ok.')

        return_code, wheel_joints_handle[3] = vrep_sim.simxGetObjectHandle(clientID, 'rollingJoint_rr', vrep_sim.simx_opmode_blocking)
        if (return_code == vrep_sim.simx_return_ok):
            print('get object youBot rollingJoint_fr ok.')
        
        # 准备五个手臂关节的初始值
        arm_joints_handle = [-1,-1,-1,-1,-1]
        for i in range(0,4):
            return_code, arm_joints_handle[i] = vrep_sim.simxGetObjectHandle(clientID, 'youBotArmJoint' + str(i), vrep_sim.simx_opmode_blocking)
            if (return_code == vrep_sim.simx_return_ok):
                print('get object arm joint ' + str(i) + ' ok.')
        
        # 初始化所需的关节位置
        desired_arm_joint_angles = [180*math.pi/180, 30.91*math.pi/180, 52.42*math.pi/180, 72.68*math.pi/180, 0]
        
        # 初始化所有手臂关节
        for i in range(0,4):
            vrep_sim.simxSetJointPosition(clientID, arm_joints_handle[i], desired_arm_joint_angles[i], vrep_sim.simx_opmode_blocking)
    else:
        print ('Failed connecting to remote API server')


    # youBot的尺码信息
    wheel_R = 0.05
    a = 0.165
    b = 0.228

    # 用户变量
    simu_time = 0
    center_velocity = [0,0,0]
    desired_wheel_velocities = [0,0,0,0]

    ''' 主循环 '''
    print('begin main control loop ...')
    while True:
        # 运动规划
        simu_time = simu_time + 0.05
        
        for i in range(0,5):
            if int(simu_time) % 2 == 0:
                desired_arm_joint_angles[i] = desired_arm_joint_angles[i] - 0.04 # rad
            else:
                desired_arm_joint_angles[i] = desired_arm_joint_angles[i] + 0.04 # rad

        # 控制youBot机器人
        VREP_armControl(vrep_sim, clientID, arm_joints_handle, desired_arm_joint_angles)
        VREP_wheelsControl(vrep_sim, clientID, wheel_joints_handle, desired_wheel_velocities)


    # 现在以非阻塞方式向 CoppeliaSim 发送一些数据：
    vrep_sim.simxAddStatusbarMessage(clientID,'Over!',vrep_sim.simx_opmode_oneshot)

    # 在关闭与 CoppeliaSim 的连接之前，请确保发出的最后一个命令有时间到达。您可以通过（例如）保证这一点：
    vrep_sim.simxGetPingTime(clientID)

    # 现在关闭与 CoppeliaSim 的连接：
    vrep_sim.simxFinish(clientID)
    print ('Program ended')

参考：

【V-REP自学笔记（七）】Matlab/Python远程控制_博士的沙漏的专栏-CSDN博客