键盘控制机械臂末端移动原理为，设置机械臂逆运动学target，机械臂末端跟随target运动，然后通过改变target的值，从而达到控制机械臂末端移动的原理。
1.第一步，首先将末端设置为跟随target运动，target为一个dummy。此时移动target，机械臂末端就会跟随target运动。如何设置不再做介绍，可以参考本文另一篇文章。
V-REP仿真-逆运动学模块的机械臂轨迹规划
2.定义函数
def move_x(self, length):
clientID = self.clientID
ikTipHandle = self.ikTipHandle
targetPosition = self.targetPosition
sim.simxSetObjectPosition(clientID, ikTipHandle, -1,targetPosition, sim.simx_opmode_oneshot)
targetPosition[0] = targetPosition[0] - length
self.jointConfig = targetPosition
def move_xx(self, length):
clientID = self.clientID
ikTipHandle = self.ikTipHandle
targetPosition = self.targetPosition
sim.simxSetObjectPosition(clientID, ikTipHandle, -1,targetPosition, sim.simx_opmode_oneshot)
targetPosition[0] = targetPosition[0] + length
self.jointConfig = targetPosition
此函数的意思为，设置target的坐标x减一个length的长度或者加一个长度。
注意，不同定义函数之间，调用ikTipHandle和targetPosition值，需要先声明。
总代码：
#-*- 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
N，M：末端的x方面运动
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
print(jointHandle[i])
_, 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)
time.sleep(2)
#读取tip和target的句柄
_, ikTipHandle = sim.simxGetObjectHandle(clientID, 'targetxin', sim.simx_opmode_blocking)
print('ikTipHandle:')
print(ikTipHandle)
_, connectionHandle = sim.simxGetObjectHandle(clientID, 'UR3_connection', sim.simx_opmode_blocking)
print('connectionHandle')
print(connectionHandle)
#读取ur3_target的位置和四元数
_, ikTipHandle_targetxin = sim.simxGetObjectHandle(clientID, 'ur3_target', sim.simx_opmode_blocking)
errorCode, targetPosition = sim.simxGetObjectPosition(clientID, ikTipHandle_targetxin, -1, sim.simx_opmode_blocking)
print(targetPosition)
errorCode, targetPosition111 = sim.simxGetObjectQuaternion(clientID, ikTipHandle_targetxin, -1, sim.simx_opmode_blocking)
print(targetPosition111)
#设置target等位置######
########
########
array_position = [-0.27789580821990967, 0.00017961859703063965, 0.4513170123100281]
sim.simxSetObjectPosition(clientID, ikTipHandle_targetxin, -1,array_position, sim.simx_opmode_oneshot)
print(targetPosition)
# 读取每个关节角度
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])
self.clientID = clientID
self.jointHandle = jointHandle
self.rgHandle = rgHandle
self.cameraRGBHandle = cameraRGBHandle
self.cameraDepthHandle = cameraDepthHandle
self.ikTipHandle = ikTipHandle
self.targetPosition = targetPosition
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.simxSetJointPosition(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.simxSetJointPosition(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.simxSetJointPosition(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) #关闭连接
def move_x(self, length):
clientID = self.clientID
ikTipHandle = self.ikTipHandle
targetPosition = self.targetPosition
sim.simxSetObjectPosition(clientID, ikTipHandle, -1,targetPosition, sim.simx_opmode_oneshot)
targetPosition[0] = targetPosition[0] - length
self.jointConfig = targetPosition
def move_xx(self, length):
clientID = self.clientID
ikTipHandle = self.ikTipHandle
targetPosition = self.targetPosition
sim.simxSetObjectPosition(clientID, ikTipHandle, -1,targetPosition, sim.simx_opmode_oneshot)
targetPosition[0] = targetPosition[0] + length
self.jointConfig = targetPosition
# control robot by keyboard
def main():
robot = UR3_RG2()
resolutionX = robot.resolutionX
resolutionY = robot.resolutionY
#angle = float(eval(input("please input velocity: ")))
angle = 5
length = 0.01
#robot.initialize_target_position_tracking()
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()
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: ")))
elif event.key == pygame.K_n:
robot.move_x(length)
elif event.key == pygame.K_m:
robot.move_xx(length)
else:
print("Invalid input, no corresponding function for this key!")
if __name__ == '__main__':
main()