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 0A,S: joint 1Z,X: joint 2E,R: joint 3D,F: joint 4C,V: joint 5P: exit()T: close RG2Y: open RG2L: reset robotSPACE: save image
"""import os
import cv2
import sys
import math
import time
import random
import string
import pygame
import sim
import numpy as npclass UR3_RG2:# variatesresolutionX = 640               # Camera resolution: 640*480resolutionY = 480joint_angle = [0,0,0,0,0,0]     # each angle of jointRAD2DEG = 180 / math.pi         # transform radian to degrees# Handles informationjointNum = 6baseName = 'UR3'rgName = 'RG2'jointName = 'UR3_joint'camera_rgb_Name = 'kinect_rgb'camera_depth_Name = 'kinect_depth'# communication and read the handlesdef __init__(self):jointNum = self.jointNumbaseName = self.baseNamergName = self.rgNamejointName = self.jointNamecamera_rgb_Name = self.camera_rgb_Namecamera_depth_Name = self.camera_depth_Nameprint('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 scriptexcept: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] = jposself.clientID = clientIDself.jointHandle = jointHandleself.rgHandle = rgHandleself.cameraRGBHandle = cameraRGBHandleself.cameraDepthHandle = cameraDepthHandleself.jointConfig = jointConfigdef __del__(self):clientID = self.clientIDsim.simxFinish(clientID)print('Simulation end')# show Handles informationdef showHandles(self):RAD2DEG = self.RAD2DEGjointNum = self.jointNumclientID = self.clientIDjointHandle = self.jointHandlergHandle = self.rgHandlecameraRGBHandle = self.cameraRGBHandlecameraDepthHandle = self.cameraDepthHandleprint('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 angledef showJointAngles(self):RAD2DEG = self.RAD2DEGjointNum = self.jointNumclientID = self.clientIDjointHandle = self.jointHandlefor i in range(jointNum):_, jpos = sim.simxGetJointPosition(clientID, jointHandle[i], sim.simx_opmode_blocking)print(round(float(jpos) * RAD2DEG, 2))print('\n')# open rg2def openRG2(self):rgName = self.rgNameclientID = self.clientIDres, retInts, retFloats, retStrings, retBuffer = sim.simxCallScriptFunction(clientID, rgName,\sim.sim_scripttype_childscript,'rg2Open',[],[],[],b'',sim.simx_opmode_blocking)# close rg2def closeRG2(self):rgName = self.rgNameclientID = self.clientIDres, 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.clientIDjointNum = self.jointNumRAD2DEG = self.RAD2DEGjointHandle = 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.clientIDRAD2DEG = self.RAD2DEGjointHandle = self.jointHandlejointConfig = self.jointConfigsim.simxSetJointTargetPosition(clientID, jointHandle[num], (jointConfig[num]+angle)/RAD2DEG, sim.simx_opmode_oneshot)jointConfig[num] = jointConfig[num] + angleself.jointConfig = jointConfig# 将第num个关节反转angle度def rotateCertainAngleNegative(self, num, angle):clientID = self.clientIDRAD2DEG = self.RAD2DEGjointHandle = self.jointHandlejointConfig = self.jointConfigsim.simxSetJointTargetPosition(clientID, jointHandle[num], (jointConfig[num]-angle)/RAD2DEG, sim.simx_opmode_oneshot)jointConfig[num] = jointConfig[num] - angleself.jointConfig = jointConfigdef StopSimulation(self):clientID = self.clientIDsim.simxStopSimulation(clientID, sim.simx_opmode_blocking)    #关闭仿真sim.simxFinish(clientID)   #关闭连接# control robot by keyboard
def main():robot = UR3_RG2()resolutionX = robot.resolutionXresolutionY = robot.resolutionY#angle = float(eval(input("please input velocity: ")))angle = 5pygame.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 0elif event.key == pygame.K_q:robot.rotateCertainAnglePositive(0, angle)elif event.key == pygame.K_w:robot.rotateCertainAngleNegative(0, angle)# joinit 1elif event.key == pygame.K_a:robot.rotateCertainAnglePositive(1, angle)elif event.key == pygame.K_s:robot.rotateCertainAngleNegative(1, angle)# joinit 2elif event.key == pygame.K_z:robot.rotateCertainAnglePositive(2, angle)elif event.key == pygame.K_x:robot.rotateCertainAngleNegative(2, angle)# joinit 3elif event.key == pygame.K_e:robot.rotateCertainAnglePositive(3, angle)elif event.key == pygame.K_r:robot.rotateCertainAngleNegative(3, angle)# joinit 4elif event.key == pygame.K_d:robot.rotateCertainAnglePositive(4, angle)elif event.key == pygame.K_f:robot.rotateCertainAngleNegative(4, angle)# joinit 5elif event.key == pygame.K_c:robot.rotateCertainAnglePositive(5, angle)elif event.key == pygame.K_v:robot.rotateCertainAngleNegative(5, angle)# close RG2elif event.key == pygame.K_t:robot.closeRG2()# # open RG2elif event.key == pygame.K_y:robot.openRG2()# reset angleelif 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()

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

V-REP仿真Python控制机械臂和rg2夹爪相关推荐

python控制机械臂6轴_基于Firmata协议的ROS Moveit六轴机械臂设计
ROS(Robot Operating System,开源机器人操作系统)是目前较为火热的一个一个机器人开发者平台.依赖于强大的资源库和开发者社区,ROS可以说是风靡全球. 在机器人开发的领域,机械臂 ...
python控制机械臂6轴_在ROS环境下，怎么使用moveit!来驱动真实的六轴机械臂？
很多小伙伴在使用ROS的时候,都会产生类似的疑问,程序写过那么多,仿真也跑过不少,但是如何控制真实机械臂/机器人呢? 今天古月君就来尝试破个题. 首先,解决这个问题的关键词是"接口" ...
v-rep仿真之键盘控制机械臂末端移动
v-rep仿真之键盘控制机械臂末端移动键盘控制机械臂末端移动原理为,设置机械臂逆运动学target,机械臂末端跟随target运动,然后通过改变target的值,从而达到控制机械臂末端移动的原理. ...
python机械臂仿真_使用VTK与Python实现机械臂三维模型可视化
三维可视化系统的建立依赖于三维图形平台, 如 OpenGL.VTK.OGRE.OSG等, 传统的方法多采用OpenGL进行底层编程,即对其特有的函数进行定量操作, 需要开发人员熟悉相关函数, 从而造成 ...
python代码控制机械臂_ros平台下python脚本控制机械臂运动
在使用moveit_setup_assistant生成机械臂的配置文件后可以使用roslaunch demo.launch启动demo,在rviz中可以通过拖动机械臂进行运动学正逆解/轨迹规划等仿真运 ...
python 机械臂控制_从零开始的ROS四轴机械臂控制-gazebo仿真控制
这是一个四轴器械臂练手项目,定为arm0.1版本,使用MG90s舵机来搭建一个四轴机械臂.arm0.1版本的目标是对带颜色的方块进行识别并在Gazebo中模拟出来. 以下是这个ROS四轴机械臂控制的目 ...
实际的机械臂控制（9）Moveit单独控制机械臂末端在XYZ三个轴的平移和旋转(基于python)
0. 序言在moveit中,控制机械臂的末端执行器运动的API有两个,分别是: shift_pose_target set_pose_target 第一个API:shift_pose_target ...
Python ROS键盘控制机械臂
Python ROS键盘控制机械臂机械臂定义所有的全局变量,给他们一个初始值 import rospy import os import sys, select, termios, tty impo ...
grbl控制3轴机械臂原理实现（三）之如何通过步进电机控制机械臂、插补算法
参考: 图形学入门 https://www.zhihu.com/question/20330720 https://zhuanlan.zhihu.com/p/30553006 https://www. ...

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

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

V-REP仿真Python控制机械臂和rg2夹爪相关推荐

最新文章

热门文章