V-REP仿真Python控制机械臂和rg2夹爪

V-REP仿真Python控制机械臂和rg2夹爪

本论文转自(作者：lanlande）：
V-rep机器人仿真(Win10)：UR5+RG2+Kinect+YOLOV3+DDPG+Pytorch(第三部分：在V-rep中用python控制机械臂)

以下代码对lanlande作者代码删减并且稍作修改，通过键盘按键控制v-rep中ur机械臂和夹爪

#-*- coding:utf-8 -*-

"""
keyboard Instructions:
    robot moving velocity: <=5(advise)
    Q,W: joint 0
    A,S: joint 1
    Z,X: joint 2
    E,R: joint 3
    D,F: joint 4
    C,V: joint 5
    P: exit()
    T: close RG2
    Y: open RG2
    L: reset robot
    SPACE: save image
"""

import os
import cv2
import sys
import math
import time
import random
import string
import pygame
import sim
import numpy as np

class UR3_RG2:
    # variates
    resolutionX = 640               # Camera resolution: 640*480
    resolutionY = 480
    joint_angle = [0,0,0,0,0,0]     # each angle of joint
    RAD2DEG = 180 / math.pi         # transform radian to degrees
    
    # Handles information
    jointNum = 6
    baseName = 'UR3'
    rgName = 'RG2'
    jointName = 'UR3_joint'
    camera_rgb_Name = 'kinect_rgb'
    camera_depth_Name = 'kinect_depth'

    # communication and read the handles
    def __init__(self):
        jointNum = self.jointNum
        baseName = self.baseName
        rgName = self.rgName
        jointName = self.jointName
        camera_rgb_Name = self.camera_rgb_Name
        camera_depth_Name = self.camera_depth_Name
        
        print('Simulation started')

        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")




        # 读取Base和Joint的句柄
        jointHandle = np.zeros((jointNum, 1), dtype=np.int)
        for i in range(jointNum):
            _, returnHandle = sim.simxGetObjectHandle(clientID, jointName + str(i+1), sim.simx_opmode_blocking)
            jointHandle[i] = returnHandle

        _, baseHandle = sim.simxGetObjectHandle(clientID, baseName, sim.simx_opmode_blocking)
        _, rgHandle = sim.simxGetObjectHandle(clientID, rgName, sim.simx_opmode_blocking)
        _, cameraRGBHandle = sim.simxGetObjectHandle(clientID, camera_rgb_Name, sim.simx_opmode_blocking)
        _, cameraDepthHandle = sim.simxGetObjectHandle(clientID, camera_depth_Name, sim.simx_opmode_blocking)
        
        # 读取每个关节角度
        jointConfig = np.zeros((jointNum, 1))
        for i in range(jointNum):
             _, jpos = sim.simxGetJointPosition(clientID, jointHandle[i], sim.simx_opmode_blocking)
             jointConfig[i] = jpos
             
        self.clientID = clientID
        self.jointHandle = jointHandle
        self.rgHandle = rgHandle
        self.cameraRGBHandle = cameraRGBHandle
        self.cameraDepthHandle = cameraDepthHandle
        self.jointConfig = jointConfig
        
    def __del__(self):
        clientID = self.clientID
        sim.simxFinish(clientID)
        print('Simulation end')
        
    # show Handles information
    def showHandles(self):
        
        RAD2DEG = self.RAD2DEG
        jointNum = self.jointNum
        clientID = self.clientID
        jointHandle = self.jointHandle
        rgHandle = self.rgHandle
        cameraRGBHandle = self.cameraRGBHandle
        cameraDepthHandle = self.cameraDepthHandle
        
        print('Handles available!')
        print("==============================================")
        print("Handles:  ")
        for i in range(len(jointHandle)):
            print("jointHandle" + str(i+1) + ": " + jointHandle[i])
        print("rgHandle:" + rgHandle)
        print("cameraRGBHandle:" + cameraRGBHandle)
        print("cameraDepthHandle:" + cameraDepthHandle)
        print("===============================================")
        
    # show each joint's angle
    def showJointAngles(self):
        RAD2DEG = self.RAD2DEG
        jointNum = self.jointNum
        clientID = self.clientID
        jointHandle = self.jointHandle
        
        for i in range(jointNum):
            _, jpos = sim.simxGetJointPosition(clientID, jointHandle[i], sim.simx_opmode_blocking)
            print(round(float(jpos) * RAD2DEG, 2))
        print('\n')
        
    # open rg2
    def openRG2(self):
        rgName = self.rgName
        clientID = self.clientID
        res, retInts, retFloats, retStrings, retBuffer = sim.simxCallScriptFunction(clientID, rgName,\
                                                        sim.sim_scripttype_childscript,'rg2Open',[],[],[],b'',sim.simx_opmode_blocking)
        
    # close rg2
    def closeRG2(self):
        rgName = self.rgName
        clientID = self.clientID
        res, retInts, retFloats, retStrings, retBuffer = sim.simxCallScriptFunction(clientID, rgName,\
                                                        sim.sim_scripttype_childscript,'rg2Close',[],[],[],b'',sim.simx_opmode_blocking)
        
    # joint_angle是这种形式: [0,0,0,0,0,0], 所有的关节都旋转到对应的角度
    def rotateAllAngle(self, joint_angle):
        clientID = self.clientID
        jointNum = self.jointNum
        RAD2DEG = self.RAD2DEG
        jointHandle = self.jointHandle
        
        # 暂停通信，用于存储所有控制命令一起发送
        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)
        
        self.jointConfig = joint_angle
        
    # 将第num个关节正转angle度
    def rotateCertainAnglePositive(self, num, angle):
        clientID = self.clientID
        RAD2DEG = self.RAD2DEG
        jointHandle = self.jointHandle
        jointConfig = self.jointConfig
        
        sim.simxSetJointTargetPosition(clientID, jointHandle[num], (jointConfig[num]+angle)/RAD2DEG, sim.simx_opmode_oneshot)
        jointConfig[num] = jointConfig[num] + angle
        
        self.jointConfig = jointConfig
        
    # 将第num个关节反转angle度
    def rotateCertainAngleNegative(self, num, angle):
        clientID = self.clientID
        RAD2DEG = self.RAD2DEG
        jointHandle = self.jointHandle
        jointConfig = self.jointConfig
        
        sim.simxSetJointTargetPosition(clientID, jointHandle[num], (jointConfig[num]-angle)/RAD2DEG, sim.simx_opmode_oneshot)
        jointConfig[num] = jointConfig[num] - angle
        
        self.jointConfig = jointConfig


    def StopSimulation(self):
        clientID = self.clientID
        sim.simxStopSimulation(clientID, sim.simx_opmode_blocking)    #关闭仿真
        sim.simxFinish(clientID)   #关闭连接
        
    
# control robot by keyboard
def main():
    robot = UR3_RG2()
    resolutionX = robot.resolutionX
    resolutionY = robot.resolutionY
    
    #angle = float(eval(input("please input velocity: ")))
    angle = 5
    
    pygame.init()
    screen = pygame.display.set_mode((resolutionX, resolutionY))
    screen.fill((255,255,255))
    pygame.display.set_caption("Vrep yolov3 ddpg pytorch")
    # 循环事件，按住一个键可以持续移动
    pygame.key.set_repeat(200,50)


    robot.closeRG2()

    time.sleep(1)

    robot.openRG2()

    time.sleep(1)

    robot.closeRG2()

    time.sleep(1)

    robot.openRG2()
    
    while True:
       
        #robot.arrayToDepthImage()
        #ig = pygame.image.load("imgTempDep\\frame.jpg")
        pygame.display.update()
        
        key_pressed = pygame.key.get_pressed()
        for event in pygame.event.get():
            # 关闭程序
            if event.type == pygame.QUIT:
                sys.exit()
            if event.type == pygame.KEYDOWN:
                if event.key == pygame.K_p:
                    robot.StopSimulation()
                    sys.exit()
                # joinit 0
                elif event.key == pygame.K_q:
                    robot.rotateCertainAnglePositive(0, angle)
                elif event.key == pygame.K_w:
                    robot.rotateCertainAngleNegative(0, angle)
                # joinit 1
                elif event.key == pygame.K_a:
                    robot.rotateCertainAnglePositive(1, angle)
                elif event.key == pygame.K_s:
                    robot.rotateCertainAngleNegative(1, angle)
                # joinit 2
                elif event.key == pygame.K_z:
                    robot.rotateCertainAnglePositive(2, angle)
                elif event.key == pygame.K_x:
                    robot.rotateCertainAngleNegative(2, angle)
                # joinit 3
                elif event.key == pygame.K_e:
                    robot.rotateCertainAnglePositive(3, angle)
                elif event.key == pygame.K_r:
                    robot.rotateCertainAngleNegative(3, angle)
                # joinit 4
                elif event.key == pygame.K_d:
                    robot.rotateCertainAnglePositive(4, angle)
                elif event.key == pygame.K_f:
                    robot.rotateCertainAngleNegative(4, angle)
                # joinit 5
                elif event.key == pygame.K_c:
                    robot.rotateCertainAnglePositive(5, angle)
                elif event.key == pygame.K_v:
                    robot.rotateCertainAngleNegative(5, angle)
                # close RG2
                elif event.key == pygame.K_t:
                    robot.closeRG2()
                # # open RG2
                elif event.key == pygame.K_y:
                    robot.openRG2()
                # reset angle
                elif event.key == pygame.K_l:
                    robot.rotateAllAngle([0,0,0,0,0,0])
                    angle = float(eval(input("please input velocity: ")))
                else:
                    print("Invalid input, no corresponding function for this key!")
                    
if __name__ == '__main__':
    main()

注意，此方法一定要在弹出的框内按键控制机械臂。