V-REP仿真之Python读取关节当前角度和驱动ur机械臂关节旋转_coppeliasim如何控制ur3机械臂采用python编写

V-REP仿真之Python读取关节当前角度和驱动ur机械臂关节旋转

在建立与机械臂的通信之后，接着需要读取和驱动ur机械臂的关节角度

代码如下：

# -*- coding:UTF-8 -*-
import sim
import time
import sys
import math
import numpy as np

try:
    sim.simxFinish(-1) #关掉之前连接
    clientID=sim.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to CoppeliaSim 
    if clientID!=-1:
        print ('connect successfully')
    else:
        sys.exit("Error: no se puede conectar") #Terminar este script
except:
    print('Check if CoppeliaSim is open')

sim.simxStartSimulation(clientID, sim.simx_opmode_blocking)     #启动仿真
print("Simulation start")
#####检索对象句柄
jointNum = 6
jointName = 'UR3_joint'
baseName = 'UR3'
jointHandle = np.zeros((jointNum,), dtype=np.int)  # 注意是整型
for i in range(jointNum):
    errorCode, returnHandle = sim.simxGetObjectHandle(clientID, jointName + str(i + 1), sim.simx_opmode_blocking)
    print(errorCode, returnHandle)
    jointHandle[i] = returnHandle
    #print(jointHandle[i])


# 读取每个关节角度
jointNum = 6
jointConfig = np.zeros((jointNum, 1))
for i in range(jointNum):
    _, jpos = sim.simxGetJointPosition(clientID, jointHandle[i], sim.simx_opmode_blocking)
    jointConfig[i] = jpos
    print(jointConfig[i])


# 驱动ur机械臂关节旋转角度
joint_angle = [0,0,0,0,0,0]     #机械臂每一个关节的旋转角度
RAD2DEG = 180 / math.pi         # transform radian to degrees

sim.simxPauseCommunication(clientID, True)
for i in range(jointNum):
    sim.simxSetJointTargetPosition(clientID, jointHandle[i], joint_angle[i]/RAD2DEG, sim.simx_opmode_oneshot)
sim.simxPauseCommunication(clientID, False)
        
#jointConfig = joint_angle

# 将第num个关节正转angle度
num = 2     #第num关节旋转
angle = 90    #第num关节旋转的角度值
sim.simxSetJointTargetPosition(clientID, jointHandle[num], (jointConfig[num]+angle)/RAD2DEG, sim.simx_opmode_oneshot)
jointConfig[num] = jointConfig[num] + angle
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56

1.检索对象句柄

#####检索对象句柄
jointNum = 6
jointName = 'UR3_joint'
baseName = 'UR3'
jointHandle = np.zeros((jointNum,), dtype=np.int)  # 注意是整型
for i in range(jointNum):
    errorCode, returnHandle = sim.simxGetObjectHandle(clientID, jointName + str(i + 1), sim.simx_opmode_blocking)
    print(errorCode, returnHandle)
    jointHandle[i] = returnHandle
    #print(jointHandle[i])
1
2
3
4
5
6
7
8
9
10

2.读取关节角度

# 读取每个关节角度
jointNum = 6
jointConfig = np.zeros((jointNum, 1))
for i in range(jointNum):
    _, jpos = sim.simxGetJointPosition(clientID, jointHandle[i], sim.simx_opmode_blocking)
    jointConfig[i] = jpos
    print(jointConfig[i])
1
2
3
4
5
6
7

3.驱动ur机械臂的6个关节

# 驱动ur机械臂关节旋转角度
joint_angle = [0,0,0,0,0,0]     #机械臂每一个关节的旋转角度
RAD2DEG = 180 / math.pi         # transform radian to degrees

sim.simxPauseCommunication(clientID, True)
for i in range(jointNum):
    sim.simxSetJointTargetPosition(clientID, jointHandle[i], joint_angle[i]/RAD2DEG, sim.simx_opmode_oneshot)
sim.simxPauseCommunication(clientID, False)
1
2
3
4
5
6
7
8

如图为机械臂的每一个关节旋转45度后的值

4.驱动ur机械臂的某一个关节旋转角度

# 将第num个关节正转angle度
num = 2     #第num关节旋转
angle = 90    #第num关节旋转的角度值
sim.simxSetJointTargetPosition(clientID, jointHandle[num], (jointConfig[num]+angle)/RAD2DEG, sim.simx_opmode_oneshot)
jointConfig[num] = jointConfig[num] + angle
1
2
3
4
5