CoppeliaSim的机械臂仿真

一、项目简介与开发依赖

​ 本项目使用Python脚本对RealMan提供的模型进行仿真，实现物体搬运的功能。

​ 项目依赖

​ Windows操作系统

​ coppeliasim4.1edu

​ Python3.9

​ 官方提供开源的sim的Python库

​ REALMAN的r65带夹爪的urdf模型

代码运行：直接运行Python脚本即可

二、关于机械臂的物理建模

2.1关于urdf文件的导入

刚开始导入urdf文件时，出现了只导入了只导入了文件中的模型关系，而没有将模型导入，查阅之后发现导入urdf文件时，urdf中的STL文件需要修改文件路径，因为出现了路径重复的情况。最后将urdf文件导入到.ttt文件当中。

P31)ORKGZ3VM9BQSPD]D@LV.png

2.2关于RealMan机械臂urdf模型物理模型动力学参数

​ 机械臂的动力学参数是十分重要的一点，它也是困扰我了将近一个月的时间，从仿真时出现抖动的问题的时候，查阅资料寻找原因，最后才发现了物理模型的重要指标，动力学参数，他会直接影响到仿真时出现抖动、运动时出现不合理移动的情况。在coppelliasim中可以将导入的urdf模型中，模块的控制面板中显示其重力、惯性矩等动力学指标，而不需要在urdf文件中修改其参数。

​ 物理引擎使用的是ODE

(文档中文翻译)ODE属性

与Open Dynamics Engine相关的属性。 请确保还参考ODE用户手册以了解详细信息。

ODE时间步长：指定动力学计算所需的时间步长。 强烈建议将时间步长保持为5ms（默认值）结合50ms的仿真时间步长，每遍模拟将得出10个动力学计算步骤。
使用’quickStep’：选择后，将使用快速迭代求解方法。 QuickStep迭代属性越大，计算（通常）就越精确。 如果未选择quickStep方法，则对于小型系统，计算可以更精确，更快。 但是，较大的系统可能会非常缓慢，不稳定，并可能导致突然崩溃！
内部缩放：指定如何在内部处理尺寸。 如果动态场景是由较小的形状（❤️ cm）或很大的形状组成的，则可以通过调整比例因子来提高仿真稳定性。 启用“完全缩放”时，将缩放与尺寸相关的值以及与质量相关的值，否则，将仅缩放与尺寸相关的值。
全局ERP：全局减少错误参数，有关更多详细信息，请参阅ODE文档。
全局CFM：全局约束力混合，有关更多详细信息，请参考ODE文档。

接下来，说明一下本六轴机械臂中，关节以及每个关节两边的机械臂模型的处理：

]GBL4YXC}B69K$2}4QU3J)M.png

2.2.1关于机械臂基座的处理

​ 刚开始做模型仿真的时候，运行机械臂他会处理倒掉的情况，处理这一情况我在底座与地板之间加了一个链接器，将其与地板连接。如图所示：

（3.22.ttt）版本，realman未提供机械爪，自己组装的机械爪

image.png

image.png

后来接触到了动力学模型，取消其底座的动力学模型，仿真时，底座就不会出现倒地的情况。

image.png

2.2.2六个关节配置的处理

因为之后要用到API库中提供的IK逆运动学模型，需要对关节运动模型修改为Inverse Kinematics mode ,混合方式操作。通过修改Upper velocity limit修改关节运动的最快速度。结合pid可以实现更精准的控制，本次仿真中没有对pid进行处理。

调整比例增益 (Kp)：增大Kp可以加快响应速度，但过大会导致系统振荡。

调整积分增益 (Ki)：增大Ki可以消除稳态误差，但过大会导致系统不稳定。

调整微分增益 (Kd)：增大Kd可以减少振荡和超调，但过大会导致系统过度响应噪声。

PID控制器的输出计算公式如下：

其中：

  • u(t)u(t)u(t) 是控制器的输出（电机指令）。
  • e(t)e(t)e(t) 是当前误差（目标位置与当前实际位置的差）。
  • KpK_pKp 是比例增益。
  • KiK_iKi 是积分增益。
  • KdK_dKd 是微分增益。

image.png

2.2.3关于关节两端响应模型的处理

关节两端的处理对机械臂的运动十分重要，如果不做处理，仿真运动会很奇怪，同学使用别的软件进行仿真时，有专门的api来取消重力造成的影响，但是那样取消重力影响来做仿真，感觉不太合理。

image.png

动力学参数对机械臂运动的影响

  • 质量（Mass）：

    • 链接的质量影响惯性和加速度。质量过大会导致运动迟缓，质量过小会导致控制不稳定。

  • 惯性矩（Inertia Matrix）：

    • 惯性矩决定了物体对旋转运动的抵抗程度。惯性矩设置不当会导致运动的抖动和不稳定。

常见形状的惯性矩公式

  1. 均匀长方体：

    • 质量 m

    • 宽度 w，高度 h，深度 d

    • 质心惯性矩：

==urdf中的质量、惯性矩参数仿真时会出现抖动，要根据仿真具体情况调整这些参数==

我每个关节按照运动一点一点调的！根据公式，再进行微调。也可能是没有学习到真实是如何实现的。

三、关于Python脚本与仿真软件的通信

3.1 关于coppeliasim通讯情况

​ 基于B0的远程API：这表示远程API的第二个版本。 它基于BlueZero中间件及其CoppeliaSim的接口插件。 与传统的远程API相比，它更易于使用且更具灵活性，最重要的是，它易于扩展。 目前，它支持以下语言：C ++，Java，Python，Matlab和Lua。

​ 旧版远程API（或简称为远程API）：这表示远程API的第一个版本。 与基于B0的远程API相比，它相对较轻并且具有更少的依赖性。 但是，它不那么直观，也不灵活，并且很难扩展。 它支持以下语言：C / C ++，Java，Python，Matlab，Octave和Lua。

​ 本次项目使用的是新版的API,具体原因如下：

1 使用旧版API时，会发现sim库中不存在某些函数。例如：根据物品handle获取物品的名称的函数。在用Python脚本语言控制机械臂时，需要不断的对物体的句柄进行操作，所有相对于旧版api来说，新版的api实用性、以及契合性跟匹配这次项目。
2 新库有些函数能比旧库返回更多的参数，可以更加方便调试。

3.2coppeliasim与Python通讯功能的实现

关于通讯的实现：

1.在软件的机械臂的线程中，调佣api，填写端口号，作为服务端，与Python脚本进行通讯

1
simRemoteApi.start(19997)

2.在Python，作为客户端，与软件进行通讯

1
2
3
4
5
6
7
8
9
10
11
12
13
14
    sim.simxFinish(-1)  # 关掉之前连接
    self.clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5)  # Connect to CoppeliaSim
    if self.clientID != -1:
        print('connect successfully')
    else:
        sys.exit("Error: no se puede conectar")  # Terminar este script

except Exception as e:
    print(f'Check if CoppeliaSim is open: {e}')
    self.clientID = -1

if self.clientID != -1:
    sim.simxStartSimulation(self.clientID, sim.simx_opmode_blocking)  # 启动仿真
    print("Simulation start")

3.Python关闭连接

1
2
3
4
5
# 关闭连接
def StopSimulation(self):
    if self.clientID != -1:
        sim.simxStopSimulation(self.clientID, sim.simx_opmode_blocking)  # 关闭仿真
        sim.simxFinish(self.clientID)

image.png

四、coppeliasim中关于机械臂仿真API的学习

4.1关于handle的获取

​ 在coppeliasim中无法在Python中导入urdf对软件的中的关节进行控制，所以需要使用它新版的通讯模块，通过handle句柄读入urdf导入软件中的关节名，以及目标物体的名字到Python中。

simxGetObjectHandle (regular API equivalent: sim.getObjectHandle)

Description Retrieves an object handle based on its name.
Python synopsis number returnCode,number handle=simxGetObjectHandle(number clientID,string objectName,number operationMode)
Python parameters clientID: the client ID. refer to simxStart.objectName: name of the object.operationMode: a remote API function operation mode. Recommended operation mode for this function is simx_opmode_blocking
Python return values returnCode: a remote API function return codehandle: the handle
Other languages C/C++, Java, Matlab, Octave, Lua
4.1.1机械臂handle的获取

URDF中关节名为joint+n，基座base_link_respondable

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
   jointNum = 6
   baseName = 'base_link_respondable'
   jointName = 'joint'
self.jointNum = self.jointNum
   self.baseName = self.baseName
   self.jointName = self.jointName
   # 读取Base和Joint的句柄
   self.jointHandle = np.zeros((self.jointNum, 1), dtype=int)
   for i in range(self.jointNum):
       result, returnHandle = sim.simxGetObjectHandle(self.clientID, self.jointName + str(i + 1), sim.simx_opmode_blocking)
       if result == sim.simx_return_ok:
           self.jointHandle[i] = returnHandle
           print(f"Joint {i + 1} handle: {self.jointHandle[i]}")
           else:
               print(f"Failed to get handle for joint {i + 1}, result: {result}")

               result, self.baseHandle = sim.simxGetObjectHandle(self.clientID, self.baseName, sim.simx_opmode_blocking)
               if result != sim.simx_return_ok:
                   print(f"Failed to get base handle, result: {result}")
4.1.2机械爪handle的获取

注意handle的数据类型，整型数组，初始化为0

1
2
3
4
5
6
7
8
9
10
11
# 读取夹爪的句柄
gripperNum = 4
gripperJoints = ['gripper_link1', 'gripper_link2', 'gripper_link3', 'gripper_link4']
self.gripperHandle = np.zeros((self.gripperNum,), dtype=int)  
for i in range(self.gripperNum):
    result, returnHandle1 = sim.simxGetObjectHandle(self.clientID, self.gripperJoints[i], sim.simx_opmode_blocking)
    if result == sim.simx_return_ok:
        self.gripperHandle[i] = returnHandle1
        print(f"Gripper {i + 1} handle: {self.gripperHandle[i]}")
        else:
            print(f"Failed to get handle for {self.gripperJoints[i]}, result: {result}")
4.1.3物体句柄的获取

获取机械臂末端位置，杯子位置，以及目标位置，便于做逆运动学。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
# 读取tip和target的句柄
result, self.ikTipHandle = sim.simxGetObjectHandle(self.clientID, 'target', sim.simx_opmode_blocking)  #机械臂末端位置
if result == sim.simx_return_ok:
    print('ikTipHandle:', self.ikTipHandle)
    else:
        print(f"Failed to get ikTip handle, result: {result}")
    result, self.targetPosition = sim.simxGetObjectPosition(self.clientID, self.ikTipHandle, -1, sim.simx_opmode_blocking)
    if result == sim.simx_return_ok:
    print("targetPosition:", self.targetPosition)
        else:
            print(f"Failed to get target position, result: {result}")
# 获取杯子和目标位置的句柄
result, self.targetHandle = sim.simxGetObjectHandle(self.clientID, 'targetPos', sim.simx_opmode_blocking)  # 目标位置
if result != sim.simx_return_ok:
    print(f"Failed to get target handle, result: {result}")

result, self.CupHandle = sim.simxGetObjectHandle(self.clientID, 'Cuboid', sim.simx_opmode_blocking)  # 杯子位置
if result != sim.simx_return_ok:
    print(f"Failed to get cup handle, result: {result}")

4.2关于关节控制

关于关节控制，官方提供了两个函数simxSetJointPosition和simxSetJointTargetPosition。刚开始做时，没有注意区别，使用的是simxSetJointPosition，出现了问题，我在调用它对爪子关节进行控制时，它没有反应。听说被弃用了，我也不清楚，但我查到的是simxSetJointPosition 函数用于直接设置关节的位置。这通常用于仿真器在动态计算之前进行关节初始化或调整关节位置，而不是在实际控制机器人运动时使用。simxSetJointTargetPosition函数用于设置关节的目标位置。当关节使用位置控制（例如 PID 控制）时，这个函数用于设置关节要移动到的目标位置。

下面是这两个函数的官方的使用说明

simxSetJointPosition (regular API equivalent: sim.setJointPosition)

Description Sets the intrinsic position of a joint. May have no effect depending on the joint mode. This function cannot be used with spherical joints (use simxSetSphericalJointMatrix instead). If you want to set several joints that should be applied at the exact same time on the CoppeliaSim side, then use simxPauseCommunication. See also simxGetJointPosition and simxSetJointTargetPosition.
Python synopsis number returnCode=simxSetJointPosition(number clientID,number jointHandle,number position,number operationMode)
Python parameters clientID: the client ID. refer to simxStart.jointHandle: handle of the jointposition: position of the joint (angular or linear value depending on the joint type)operationMode: a remote API function operation mode. Recommended operation modes for this function are simx_opmode_oneshot or simx_opmode_streaming
Python return values returnCode: a remote API function return code
Other languages C/C++, Java, Matlab, Octave, Lua

simxSetJointTargetPosition (regular API equivalent: sim.setJointTargetPosition)

Description Sets the target position of a joint if the joint is in torque/force mode (also make sure that the joint’s motor and position control are enabled). See also simxSetJointPosition.
Python synopsis number returnCode=simxSetJointTargetPosition(number clientID,number jointHandle,number targetPosition,number operationMode)
Python parameters clientID: the client ID. refer to simxStart.jointHandle: handle of the jointtargetPosition: target position of the joint (angular or linear value depending on the joint type)operationMode: a remote API function operation mode. Recommended operation modes for this function are simx_opmode_oneshot or simx_opmode_streaming
Python return values returnCode: a remote API function return code
Other languages C/C++, Java, Matlab, Octave, Lua

下面是机械爪调用api使用方法，具体使用在五中说明，机械爪控制也是仿真控制上的一个难点。

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
def control_gripper(self, close_gripper=True):
    if close_gripper:
        positions = [25/180*3.1415, -25/180*3.1415, -25/180*3.1415, 25/180*3.1415]
    else:
        positions = [-10/180*3.1415, 10/180*3.1415, 10/180*3.1415, -10/180*3.1415]
        # positions = [30 if close_gripper else 0.0, -30 if close_gripper else 0.0]  # 设置夹爪的目标位置，确保角度值在合理范围内
    for i, handle in enumerate(self.gripperHandle[0:2]):
        print(f"Setting gripper {i + 1} position to {positions[0]}")
        result = sim.simxSetJointTargetPosition(self.clientID, int(handle), positions[i], sim.simx_opmode_blocking)
        if result == sim.simx_return_ok:
            print(f"Successfully set position for gripper {i + 1}")
        else:
            print(f"Failed to set position for gripper {i + 1}, result: {result}")

    for i, handle in enumerate(self.gripperHandle[2:4]):
        print(f"Setting gripper {i + 3} position to {positions[1]}")
        result = sim.simxSetJointTargetPosition(self.clientID, int(handle), positions[i+2],sim.simx_opmode_blocking)
        # print(result)
        if result == sim.simx_return_ok:
            print(f"Successfully set position for gripper {i + 3}")
        else:
            print(f"Failed to set position for gripper {i + 3}, result: {result}")
    
    if close_gripper:
        result=sim.simxSetIntegerSignal(self.clientID,"close",1,sim.simx_opmode_blocking)
    else:
        result=sim.simxSetIntegerSignal(self.clientID,"close",0,sim.simx_opmode_blocking)

4.3关于图像处理

图像处理属于我在项目要求之外自己添加的一个任务，通过在软件中调用视觉传感器，python中调用视觉句柄，使用OpenCV库进行图像识别

simxGetVisionSensorImage (regular API equivalent: sim.getVisionSensorImage)

Description Retrieves the image of a vision sensor. The returned data doesn’t make sense if sim.handleVisionSensor wasn’t called previously (sim.handleVisionSensor is called by default in the main script if the vision sensor is not tagged as explicit handling). Use the simxGetLastCmdTime function to verify the “freshness” of the retrieved data. See also simxSetVisionSensorImage, simxGetVisionSensorDepthBuffer and simxReadVisionSensor.
Python synopsis number returnCode,array resolution,array image=simxGetVisionSensorImage(number clientID,number sensorHandle,number options,number operationMode)
Python parameters clientID: the client ID. refer to simxStart.sensorHandle: handle of the vision sensoroptions: image options, bit-coded:bit0 set: each image pixel is a byte (greyscale image), otherwise each image pixel is a rgb byte-tripletoperationMode: a remote API function operation mode. Recommended operation modes for this function are simx_opmode_streaming (the first call) and simx_opmode_buffer (the following calls)
Python return values returnCode: a remote API function return coderesolution: the resolution of the image (x,y)image: the image data.
Other languages C/C++, Java, Matlab, Octave, Lua

使用案例

1
2
3
4
5
6
# 获取相机句柄
result, self.cameraHandle = sim.simxGetObjectHandle(self.clientID, 'Vision_sensor', sim.simx_opmode_blocking)
if result == sim.simx_return_ok:
    print(f"Camera handle: {self.cameraHandle}")
else:
    print(f"Failed to get camera handle, result: {result}")
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
#视觉传感器读取相机图片的使用
def readVisionSensor(self):
    global resolution
    result, resolution, c_image = sim.simxGetVisionSensorImage(self.clientID, self.cameraHandle, 0,sim.simx_opmode_blocking)
    if result == sim.simx_return_ok:
        # print("Image captured successfully")
        # print(f"Resolution: {resolution}")
        # print(f"Raw image data length: {len(c_image)}")

        if len(c_image) == 0:
            # print("Empty image data received")
            return None

        image = np.array(c_image).astype(np.uint8)  # 显式转换为 uint8
        # print(f"Image data after np.array conversion: {image[:10]}")  # 打印前10个像素值以作调试

        image.resize([resolution[1], resolution[0], 3])  # 调整图像大小
        # print(f"Image shape after resize: {image.shape}")

        image = cv2.flip(image, 0)  # 图像上下翻转
        return image
    else:
        print("Failed to capture image")
        return None
#读取相机图像
    def process_image(self, image):
        if image is not None:
            # print(f"Processing image with shape: {image.shape}")
            self.detect_object(image)
            cv2.imshow('Processed Image', image)
            cv2.waitKey(1)
            else:
                print("No image to process")

#使用线程调用
    def capture_and_process(robot):
        while True:
            image = robot.readVisionSensor()
            robot.process_image(image)
	thread1 = threading.Thread(target=capture_and_process, args=(robot,))

五、机械爪的抓取与放下的仿真

5.1通过摩擦力进行目标物体的抓取

​ 关于机械臂的抓取与放下，是最困扰我的一部分内容，处理这个问题，花费了大概一个月的时间，T-T。刚开始的时候我的想想法是运动到目标位置前方，通过前伸闭合机械爪关节进行抓取，但是在正式做的时候先是遇到了机械爪闭合不了的问题，然后遇到了闭合之后抓取物体时出现的抖动，以及在运动时出现的加速度，使得机械爪乱动的情况，物体掉落。做了一段时间后感觉这样做对于动力学参数调整的过程太过于复杂。放弃了这个方法。

image.png

这是当时写的控制部分。。。

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
# 移动到杯子上方
above_cup_position = list(cup_position)
above_cup_position[2] += 0.1  # 移动到杯子上方10cm
self.move_to_position(above_cup_position)

# 移动到抓取位置
self.move_to_position(cup_position)

# 模拟抓取动作
# 闭合夹爪抓取杯子
self.control_gripper(open_gripper=True)

time.sleep(3)  # 假设抓取需要1秒

# 抬起杯子
self.move_to_position(above_cup_position)

# 移动到目标上方
above_target_position = list(target_position)
above_target_position[2] += 0.1  # 移动到目标上方10cm
self.move_to_position(above_target_position)

# 移动到放置位置
self.move_to_position(target_position)

# 模拟释放动作
# 打开夹爪释放杯子

self.control_gripper(open_gripper=False)

time.sleep(3)  # 假设释放需要1秒

# 抬起夹爪
self.move_to_position(above_target_position)

5.2通过机械臂末端与物体的父子关系建立连接仿真抓取动作

​ 最后想到了这个办法，通过使用链接器，在机械臂末端加入一个链接器，在夹爪到达目标物体时，通过链接器将物体与末端进行连接，这个想法是我想到，我刚开始做这个项目时，使用的是基座与地板通过链接器，也就是力传感器进行连接，这样的话我想到了，抓取的时候也可以这样做，抓取只是一个做一个动作，做完之后可以直接通过力传感器将两物体进行连接，到达目标位置时，再取消连接关系，这样我就可以完成这样的抓取放下的一个动作。接下来的一周，实践了这个想法。

5.2.1抓取方式的改变

将之前的前抓，还为了下抓，这样比起前抓，有几处好处：

  1. 提高稳定性

向下抓取通常可以提供更稳定的抓取，因为重力有助于将物体固定在抓手中。这可以减少物体在抓取过程中滑动或掉落的风险。

  1. 增强力的利用

向下抓取可以更好地利用抓手的力，因为抓手可以更直接地施加垂直力。

  1. 简化抓取控制

向下抓取通常比镶嵌抓取更容易控制，因为它减少了对抓手姿态的精确调整要求。这样可以简化控制算法，并提高抓取成功率。

4.减少干涉

在某些情况下，镶嵌抓取可能会遇到周围物体的干涉，导致抓取困难。向下抓取可以减少这种干涉，因为抓手可以从上方直接进入抓取区域。

5.适应不同形状的物体

向下抓取对于不同形状的物体通常具有更好的适应性，特别是对于表面不规则或形状复杂的物体，向下抓取可以更容易地找到合适的抓取点。

6.方便之后添加功能，做视觉定位

5.2.2抓取动作的实现

​ 之前我在抓取时调用的函数为simxSetJointPosition，使用时他没有做出相应的动作，网上说可能被弃用等等，但最主要的是我之前没有区别出simxSetJointTargetPosition的不同之处。这一点我在4.1.2中已经写出原因，不过多赘述。

​ 第二点就是我在改变角度时，以为他是角度制，但是我错了，它应该是用的弧度制。

​ 第三点就是关节运动时，它不像别的软件那样设置目标点，可以缓慢运动到目标位置，他需要通过差值运动将其慢慢运动到目标位置。

具体实现代码如下所示：

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
def control_gripper(self, close_gripper=True):
    if close_gripper:
        positions = [25/180*3.1415, -25/180*3.1415, -25/180*3.1415, 25/180*3.1415]
    else:
        positions = [-10/180*3.1415, 10/180*3.1415, 10/180*3.1415, -10/180*3.1415]
        # positions = [30 if close_gripper else 0.0, -30 if close_gripper else 0.0]  # 设置夹爪的目标位置，确保角度值在合理范围内
    for i, handle in enumerate(self.gripperHandle[0:2]):
        print(f"Setting gripper {i + 1} position to {positions[0]}")
        result = sim.simxSetJointTargetPosition(self.clientID, int(handle), positions[i], sim.simx_opmode_blocking)
        if result == sim.simx_return_ok:
            print(f"Successfully set position for gripper {i + 1}")
        else:
            print(f"Failed to set position for gripper {i + 1}, result: {result}")

    for i, handle in enumerate(self.gripperHandle[2:4]):
        print(f"Setting gripper {i + 3} position to {positions[1]}")
        result = sim.simxSetJointTargetPosition(self.clientID, int(handle), positions[i+2],sim.simx_opmode_blocking)
        # print(result)
        if result == sim.simx_return_ok:
            print(f"Successfully set position for gripper {i + 3}")
        else:
            print(f"Failed to set position for gripper {i + 3}, result: {result}")
    
    if close_gripper:
        result=sim.simxSetIntegerSignal(self.clientID,"close",1,sim.simx_opmode_blocking)
    else:
        result=sim.simxSetIntegerSignal(self.clientID,"close",0,sim.simx_opmode_blocking)

realman提供给我的夹爪的handle中夹爪关节的名字为1,11,2,22。我将其改为了1,2,3,4，方便对爪子的每一方向进行控制逻辑的写法。

image.png

5.2.3 链接器（力传感器）功能的实现

最后是抓取动作的实现，对于抓取动作，我调用了simxSetIntegerSignal，它是用来给软件信号的。整形的信号，我用0/1控制。

下面是官方给的接口

simxSetIntegerSignal (regular API equivalent: sim.setIntegerSignal)

Description Sets the value of an integer signal. If that signal is not yet present, it is added. See also simxGetIntegerSignal, simxClearIntegerSignal, simxSetFloatSignal and simxSetStringSignal.
Python synopsis number returnCode=simxSetIntegerSignal(number clientID,string signalName,number signalValue,number operationMode)
Python parameters clientID: the client ID. refer to simxStart.signalName: name of the signalsignalValue: value of the signaloperationMode: a remote API function operation mode. Recommended operation mode for this function is simx_opmode_oneshot
Python return values returnCode: a remote API function return code
Other languages C/C++, Java, Matlab, Octave, Lua 
1
2
3
4
if close_gripper:
    result=sim.simxSetIntegerSignal(self.clientID,"close",1,sim.simx_opmode_blocking)
else:
    result=sim.simxSetIntegerSignal(self.clientID,"close",0,sim.simx_opmode_blocking)

在软件的末端要添加lua脚本，响应。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
function sysCall_init()
    -- do some initialization here
    sim.clearIntegerSignal("close")
    attachedShape=sim.getObjectHandle("Cuboid")
    conenctor=sim.getObjectHandle("gripper_attach")
end

function sysCall_actuation()
    -- put your actuation code here
    local data=sim.getIntegerSignal('close')
    if data and data~=0 then
        sim.setObjectParent(attachedShape,conenctor,true)
    else
        sim.setObjectParent(attachedShape,-1,true)
    end
end

链接器放置如图所示：

image.png

这样完成了抓取的动作。

六、机械臂仿真运动整体逻辑

​ 接下来是运动逻辑。

6.1关于逆运动学

1.自己写逆运动学解算，通过机器人学逆运动学相关知识，封装一个逆运动学库，但在Python脚本中写一个逆运动学库，再通过通讯实现数据的传递不现实，我们在进行逆运动学仿真时，对于实时性的要求很大，在运动过程中，它是通过插值运动，一步步的运动到目标位置，每一个时间片都需要处理一次逆运动学，在coppeliasim中，需要通过通讯才能实现数据的传递，这样逆解的传递效果会很差。

2.通过coppeliasim内置的仿真环境中的逆运动学。实现功能。

具体实现如下所示：

1、配置tip与target，末端tip为初始位置，target为目标位置，通过逆运动关系将其连接。

image.png

2.设置内置逆运动学参数

image.png

3.实现对target的跟踪。

读取target句柄。

1
2
3
4
5
6
7
8
9
10
11
12
# 读取tip和target的句柄
result, self.ikTipHandle = sim.simxGetObjectHandle(self.clientID, 'target', sim.simx_opmode_blocking) 
if result == sim.simx_return_ok:
    print('ikTipHandle:', self.ikTipHandle)
else:
    print(f"Failed to get ikTip handle, result: {result}")

result, self.targetPosition = sim.simxGetObjectPosition(self.clientID, self.ikTipHandle, -1, sim.simx_opmode_blocking)
if result == sim.simx_return_ok:
    print("targetPosition:", self.targetPosition)
else:
    print(f"Failed to get target position, result: {result}")

6.2关于运动路线

关于运动路线的实现，通过插值运动，实现target差值运动到目标物体正上方。插值运动为直线的路径设置。

1
2
3
4
5
6
dp = [ position[0]-current_position[0], position[1]-current_position[1], position[2]-current_position[2] ]
dis = math.sqrt(dp[0]*dp[0] + dp[1]*dp[1] + dp[2]*dp[2])
steps = round(dis / 0.005)
if steps <= 10:
    steps = 10

1
dp = [ position[0]-current_position[0], position[1]-current_position[1], position[2]-current_position[2] ]

​ 计算目标位置 position 与当前机械臂位置 current_position 之间的差异向量 dp。具体来说，它分别计算了目标位置和当前机械臂位置在 x、y、z 三个方向上的差异。

1
dis = math.sqrt(dp[0]*dp[0] + dp[1]*dp[1] + dp[2]*dp[2])

​ 使用欧几里得距离公式计算目标位置与当前机械臂位置之间的直线距离 dis。：

在代码中，这个公式被具体实现为：

dp[0]、dp[1]、dp[2] 分别是目标位置与当前机械臂位置在 x、y、z 三个方向上的差异。

1
steps = round(dis / 0.005)

计算插值移动的步数 steps。具体来说，它根据目标位置与当前机械臂位置之间的直线距离 dis，除以一个固定步长（这里是 0.005 米），来确定插值的步数。这个步长值可以调整，以控制移动的平滑度和精度。

  • 如果距离较大，步数较多，移动更平滑。

  • 如果距离较小，步数较少，移动更快速。

    限制最小步长

1
if steps <= 10:    steps = 10

​ 确保插值步数 steps 的最小值为 10。如果计算得到的步数 steps 小于或等于 10，则将步数设置为 10，以避免步数过少导致的移动不平滑。

通过插值运动移动到物体位置，以及目标位置。

完成整体代码的实现

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
def move_cup_to_target(self):
    # 获取杯子和目标位置
    result, cup_position = sim.simxGetObjectPosition(self.clientID, self.CupHandle, -1, sim.simx_opmode_blocking)
    if result != sim.simx_return_ok:
        print(f"Failed to get cup position, result: {result}")
        return
    
    result, target_position = sim.simxGetObjectPosition(self.clientID, self.targetHandle, -1, sim.simx_opmode_blocking)
    if result != sim.simx_return_ok:
        print(f"Failed to get target position, result: {result}")
        return

    # 移动到杯子上方
    above_cup_position = list(cup_position)
    above_cup_position[2] += 0.1  # 移动到杯子上方10cm
    self.move_to_position(above_cup_position)
    time.sleep(5.0)

    # 移动到抓取位置
    self.move_to_position(cup_position)
    time.sleep(2.0)

    # 闭合夹爪抓取杯子
    self.control_gripper(close_gripper=True)
    time.sleep(2.0)  # 假设抓取需要1秒

    # 抬起杯子
    self.move_to_position(above_cup_position)
    time.sleep(2.0)

    # 移动到目标上方
    above_target_position = list(target_position)
    above_target_position[2] += 0.1  # 移动到目标上方10cm
    self.move_to_position(above_target_position)
    time.sleep(5.0)
    
    # 移动到放置位置
    drop_target_position = list(target_position)
    drop_target_position[2] += 0.01  # 移动到目标上方10cm
    self.move_to_position(drop_target_position)
    time.sleep(2.0)
    
    # 打开夹爪释放杯子
    self.control_gripper(close_gripper=False)
    time.sleep(1.0)  # 假设抓取需要1秒

    # 抬起夹爪
    self.move_to_position(above_target_position)
    time.sleep(1.0)