1. 实现效果

2. 前言

关于在coppeliaSim中使用Python控制UR5机器人有一个开源项目供参考：
https://github.com/Junzhuodu/ur5_vrep_python

在Vrep改名为coppeliaSim之后，vrep.py更名为sim.py，vrepConst.py更名为simConst.py，remoteApi.so（Ubuntu 用户使用该文件）文件内容可能改变。在官方手册的Python API教程可以知道这三个文件必须放置在Python文件夹下。

3. 开发环境

  • Ubuntu20.04
  • coppeliaSim v4.1.0
  • Python3

4. 准备工作

文件目录如下：

.
├── gotoPosition.py
├── remoteApi.so
├── simConst.py
├── sim.py
└── ur5_pickup .ttt

场景模型中的手爪和dummy的欧拉角最好调整为如下的位置，否则轨迹规划容易出现错误！

在这里插入图片描述

在这里插入图片描述
目标点的欧拉角也进行调整，以便可以规划到该位置。
在这里插入图片描述
最后场景如下图所示：
在这里插入图片描述

5. 代码思路

开源项目中使用的是findPath_goalIsState让机器人到达指定的关节角配置以进行轨迹规划，但这显然不实用，我们更希望工具坐标中心点抵达我们指定的齐次坐标所表示的位置。
remoteApiCommandServer中还提供了另外一个findPath_goalIsPose供调用。仔细查看两个函数的内容就可以发现，函数参数inInts和inFloats的内容是不同的！

这里我们将开源代码gotoPosition.py中的：

inInts=[robotHandle,collisionChecking,minConfigsForPathPlanningPath,searchCount]

inFloats = robotCurrentConfig + [-1.0110, -1.1291, 0.7916, 0.3375, 2.1395, 0.7224]

修改为：

res,target1=sim.simxGetObjectHandle(clientID,'testPose1#',sim.simx_opmode_oneshot_wait)


inInts=[robotHandle,collisionChecking,minConfigsForIkPath,minConfigsForPathPlanningPath,maxConfigsForDesiredPose,maxTrialsForConfigSearch,searchCount]

inFloats = robotCurrentConfig + target1Pose + approachVector

这样才能正确进行轨迹规划！

如果轨迹规划失败，就需要调整目标点到合适的位置，直到可以规划出轨迹才行。

6. 代码实现

#!/usr/bin/env python

import sim

sim.simxFinish(-1)
clientID=sim.simxStart('127.0.0.1',19997,True,True,-500000,5)

if clientID!=-1:
    print ('Connected to remote API')

    sim.simxStartSimulation(clientID,sim.simx_opmode_oneshot_wait) # Start the simulation

    # Retrieve some handles
    res,robotHandle=sim.simxGetObjectHandle(clientID,'UR5#', sim.simx_opmode_oneshot_wait)
    res,target1=sim.simxGetObjectHandle(clientID,'testPose1#',sim.simx_opmode_oneshot_wait)

    emptyBuff = bytearray()
    res,retInts,target1Pose,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getObjectPose',[target1],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
    res,retInts,robotInitialState,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getRobotState',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)

    # Some parameters:
    approachVector=[0,0,0] # often a linear approach is required. This should also be part of the calculations when selecting an appropriate state for a given pose
    maxConfigsForDesiredPose=100 # we will try to find 10 different states corresponding to the goal pose and order them according to distance from initial state
    maxTrialsForConfigSearch=300 # a parameter needed for finding appropriate goal states
    searchCount=4 # how many times OMPL will run for a given task
    minConfigsForPathPlanningPath=400 # interpolation states for the OMPL path
    minConfigsForIkPath=100 # interpolation states for the linear approach path
    collisionChecking=1 # whether collision checking is on or off
    inInts=[robotHandle,collisionChecking,minConfigsForIkPath,minConfigsForPathPlanningPath,maxConfigsForDesiredPose,maxTrialsForConfigSearch,searchCount]

    res,retInts,robotCurrentConfig,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getRobotState',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
    inFloats = robotCurrentConfig + target1Pose + approachVector

    res,retInts,path,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'findPath_goalIsPose',inInts,inFloats,[],emptyBuff,sim.simx_opmode_oneshot_wait)

    if (res==0) and len(path)>0:
            # Visualize the path:
            res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'visualizePath',[robotHandle,255,0,255],path,[],emptyBuff,sim.simx_opmode_oneshot_wait)
            lineHandle=retInts[0]

            # Make the robot follow the path:
            res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'runThroughPath',[robotHandle],path,[],emptyBuff,sim.simx_opmode_oneshot_wait)

            # Wait until the end of the movement:
            runningPath=True
            while runningPath:
                res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'isRunningThroughPath',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
                runningPath=retInts[0]==1

            # Clear the path visualization:
            res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'removeLine',[lineHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)


else:
    print ('Failed connecting to remote API server')

sim.simxGetPingTime(clientID)
sim.simxFinish(clientID) # Now close the connection to V-REP
print ('Program ended')

7. 相关阅读

[1] 【Coppeliasim｜轨迹规划（一）】设置机械臂的关节角时保证机械臂不散架
[2] 【Coppeliasim｜轨迹规划（二）】不同关节角下，机械臂末端的距离
[3] 【Coppeliasim｜轨迹规划（三）】机械臂逆解
[4] 【Coppeliasim｜轨迹规划（四）】机械臂轨迹规划（避免碰撞）
[5] 【Coppeliasim｜轨迹规划（五）】给机械臂添加手爪