V-rep仿真学习（二）

爱被打了一巴掌 2022-09-11 01:26 151阅读 0赞

参考（一）中1，2文章，做了一个可以使用小键盘控制的UR3机械臂  
十分感谢两位作者。

### 文章目录 ###

*  前言
 *  一、基于UR3的逆运动学
 *  二、python部分
 *   *  1.修改机械臂脚本文件
     *  2.文件系统
     *  3.simpleTest
 *  总结

--------------------

# 前言 #

最近在做机械臂的力控制，末端装六维力传感器测接触力然后控制的那种，因为动真机比较危险，涉及到机械臂和环境的真实接触才能测的到力的数据，所以打算先在仿真里搞一搞，后面想用强化学习来做机械臂轴孔装配，也可以用仿真数据去训练，可以加vx聊一下，现在就是有想法没思路。  
之前也用过ros+gazebo，有点费劲。后来下了vrep（coppeliasim）直接把机械臂从左边拖出来就能用，直接让我把ros扔了（看了很久的ros书籍，浪费了）。

关于vrep的基础操作本文就不提了，建议看一下（一）中推荐的第二篇的作者的其他文章，讲了很多。如果你跟我一样也不喜欢看英文用户手册的话。。

--------------------

# 一、基于UR3的逆运动学 #

根据我找到的资料，就是搞两个dummy，把他俩连起来，然后一个放在最外面，一个放在机械臂末端，像这样：  
![在这里插入图片描述][watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAYXByaWxhYWFhYQ_size_20_color_FFFFFF_t_70_g_se_x_16]  
然后给ur3\_tip这个小点点加逆运动学算法。先点击ur3\_tip，点击右侧的f(x)图标，然后这样设置：

![请添加图片描述][watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAYXByaWxhYWFhYQ_size_20_color_FFFFFF_t_70_g_se_x_16 1]  
具体设置过程-------->[这篇文章][Link 1]讲的很详细。

然后ur3的每一个关节都设置一下：  
![请添加图片描述][watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAYXByaWxhYWFhYQ_size_20_color_FFFFFF_t_70_g_se_x_16 2]  
就从joint1到joint6，一路顺着点下去就ok了，把ur3的脚本文件全删干净，就是开启仿真会动一动的。  
这时候拖动target这个点，机械臂就可以做跟随运动了。

然后下面开始使用python编程移动和旋转这个点了。

# 二、python部分 #

## 1.修改机械臂脚本文件 ##

改成这句话：

simRemoteApi.start(19997)

就是开一个远程通讯的端口（我是这样理解的）。。  
![请添加图片描述][watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAYXByaWxhYWFhYQ_size_20_color_FFFFFF_t_70_g_se_x_16 3]

## 2.文件系统 ##

![在这里插入图片描述][baf22791dd6242358d4f6c6456b5ed78.png]  
目录下一定要有这么几个文件，他们具体在哪百度一下，我也忘了，反正就在你的vrep安装目录里面，找到以后拷出来就好了，sim.py是vrep提供的库，simConst.py应该也是个库，remoteApi.dll我也不知道是啥，反正要有。

我们自己的程序要在simpleTest.py里搞。

## 3.simpleTest ##

全文如下：

# -*- coding:utf-8 -*-
    
    
    import sys
    import math
    import time
    import pygame
    import sim
    import numpy as np
    
    
    class UR3_RG2:
        # variates
        resolutionX = 200   # 这里是pygame界面的窗口大小xy
        resolutionY = 200
        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'
        jointName = 'UR3_joint'
    
        # communication and read the handles
        def __init__(self):
            jointNum = self.jointNum
            baseName = self.baseName
            jointName = self.jointName
    
            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)
            time.sleep(2)
    
            # 读取tip和target的句柄
            _, ikTipHandle = sim.simxGetObjectHandle(clientID, 'target', sim.simx_opmode_blocking)
            print('ikTipHandle:')
            print(ikTipHandle)
            _, connectionHandle = sim.simxGetObjectHandle(clientID, 'UR3_connection', sim.simx_opmode_blocking)
            print('connectionHandle:')
            print(connectionHandle)
            errorCode, targetPosition = sim.simxGetObjectPosition(clientID, ikTipHandle, -1,
                                                                  sim.simx_opmode_blocking)
            print("target,targetPosition:")
            print(targetPosition)
            # 获取欧拉角
            errorCode, euler = sim.simxGetObjectOrientation(clientID, ikTipHandle, -1, 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
                # print(jointConfig[i])
    
            self.clientID = clientID
            self.jointHandle = jointHandle
            self.ikTipHandle = ikTipHandle
            self.targetPosition = targetPosition
            self.jointConfig = jointConfig
            self.euler = euler
    
        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
    
            print('Handles available!')
            print("==============================================")
            print("Handles: ")
            for i in range(len(jointHandle)):
                print("jointHandle" + str(i + 1) + ": " + jointHandle[i])
            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')
    
        def StopSimulation(self):
            clientID = self.clientID
            sim.simxStopSimulation(clientID, sim.simx_opmode_blocking)  # 关闭仿真
            sim.simxFinish(clientID)  # 关闭连接
    
        def move_x(self, length):                   # 沿着x正方向动一动
            clientID = self.clientID
            ikTipHandle = self.ikTipHandle
            targetPosition = self.targetPosition
            targetPosition[0] += length
            sim.simxSetObjectPosition(clientID, ikTipHandle, -1, targetPosition, sim.simx_opmode_oneshot)
            self.jointConfig = targetPosition
    
        def move_xx(self, length):                  # 沿着x负方向动一动
            clientID = self.clientID
            ikTipHandle = self.ikTipHandle
            targetPosition = self.targetPosition
            targetPosition[0] -= length
            sim.simxSetObjectPosition(clientID, ikTipHandle, -1, targetPosition, sim.simx_opmode_oneshot)
            self.jointConfig = targetPosition
    
        def move_y(self, length):
            clientID = self.clientID
            ikTipHandle = self.ikTipHandle
            targetPosition = self.targetPosition
            targetPosition[1] += length
            sim.simxSetObjectPosition(clientID, ikTipHandle, -1, targetPosition, sim.simx_opmode_oneshot)
            self.jointConfig = targetPosition
    
        def move_yy(self, length):
            clientID = self.clientID
            ikTipHandle = self.ikTipHandle
            targetPosition = self.targetPosition
            targetPosition[1] -= length
            sim.simxSetObjectPosition(clientID, ikTipHandle, -1, targetPosition, sim.simx_opmode_oneshot)
            self.jointConfig = targetPosition
    
        def move_z(self, length):
            clientID = self.clientID
            ikTipHandle = self.ikTipHandle
            targetPosition = self.targetPosition
            targetPosition[2] += length
            sim.simxSetObjectPosition(clientID, ikTipHandle, -1, targetPosition, sim.simx_opmode_oneshot)
            self.jointConfig = targetPosition
    
        def move_zz(self, length):
            clientID = self.clientID
            ikTipHandle = self.ikTipHandle
            targetPosition = self.targetPosition
            targetPosition[2] -= length
            sim.simxSetObjectPosition(clientID, ikTipHandle, -1, targetPosition, sim.simx_opmode_oneshot)
            self.jointConfig = targetPosition
    
        def move_rx(self, angle):                           # rx正方向转一转
            clientID = self.clientID
            ikTipHandle = self.ikTipHandle
            euler = self.euler
            euler[0] += angle
            sim.simxSetObjectOrientation(clientID, ikTipHandle, -1, euler, sim.simx_opmode_oneshot)
    
        def showtargetpositon(self):                        # 显示一下当前位置状态
            clientID = self.clientID
            ikTipHandle = self.ikTipHandle
            targetPosition = self.targetPosition
    
            _, ikTipHandle = sim.simxGetObjectHandle(clientID, 'target', sim.simx_opmode_blocking)
            errorCode, targetPosition = sim.simxGetObjectPosition(clientID, ikTipHandle, -1,
                                                                  sim.simx_opmode_blocking)
    
            print("guoguoguo")
            print(targetPosition)
    
    
    # control robot by keyboard
    def main():
        robot = UR3_RG2()
        resolutionX = robot.resolutionX
        resolutionY = robot.resolutionY
    
        # angle = float(eval(input("please input velocity: ")))
        angle = 1
        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("guo_Vrep")
        # 循环事件，按住一个键可以持续移动
        pygame.key.set_repeat(200, 50)
    
        while True:
    
            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()
    
    
    
                    # 按键部分
    
                    # 小键盘1，2对应x轴正负
                    # 小键盘4，5对应y轴正负
                    # 小键盘7，8对应z轴正负
                    # 小键盘3对应rx正
                    #
                    # 后续需要把rx ry rz加上
    
    
    
                    elif event.key == pygame.K_KP1:
                        robot.move_x(length)
                    elif event.key == pygame.K_KP2:
                        robot.move_xx(length)
    
                    elif event.key == pygame.K_KP4:
                        robot.move_y(length)
                    elif event.key == pygame.K_KP5:
                        robot.move_yy(length)
    
                    elif event.key == pygame.K_KP7:
                        robot.move_z(length)
                    elif event.key == pygame.K_KP8:
                        robot.move_zz(length)
                    elif event.key == pygame.K_KP3:
                        robot.move_rx(angle)
                    elif event.key == pygame.K_g:
                        robot.showtargetpositon()
    
    
                    else:
                        print("Invalid input, no corresponding function for this key!")
    
    
    if __name__ == '__main__':
        main()

注释写的很详细，还缺了一部分，就是rx ry rz的旋转，我只写了rx的正向旋转，如果需要旋转的话可以仿照move\_rx这个方法写，euler\[0\]改成\[1\] \[2\]就可以了。

# 总结 #

等下上传个b站视频，记录一下。  
https://www.bilibili.com/video/BV1Zb4y117Fz/

[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAYXByaWxhYWFhYQ_size_20_color_FFFFFF_t_70_g_se_x_16]: /images/20220829/947b3f42f8da4bc39bab05674148c65a.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAYXByaWxhYWFhYQ_size_20_color_FFFFFF_t_70_g_se_x_16 1]: /images/20220829/db6d449ad5ed4ad48e3d5410999e6c6f.png
[Link 1]: https://blog.csdn.net/sinat_39153249/article/details/106626985?spm=1001.2014.3001.5501
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAYXByaWxhYWFhYQ_size_20_color_FFFFFF_t_70_g_se_x_16 2]: /images/20220829/e6b1a8747a4f47e5af28ab75f108dfe4.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAYXByaWxhYWFhYQ_size_20_color_FFFFFF_t_70_g_se_x_16 3]: /images/20220829/e1909db48fe24b478d16db6db072ba7d.png
[baf22791dd6242358d4f6c6456b5ed78.png]: /images/20220829/5158637062a54768bc2746b5e5d0eb51.png