使用Python结合CoppeliaSim API来实现对UR5机械臂末端轨迹记录

以下是一个使用Python结合CoppeliaSim API来实现对UR5机械臂末端轨迹记录，同时保证机械臂末端坐标系始终竖直向下，并允许更改初始点和终点的示例代码。

实现思路

连接CoppeliaSim：使用sim.py和simConst.py文件提供的API连接到CoppeliaSim仿真环境。
获取机械臂和末端执行器的句柄：通过名称获取UR5机械臂和末端执行器的句柄。
设置初始点和终点：根据需求设置机械臂末端的初始点和终点。
规划路径：确保机械臂末端在运动过程中始终保持竖直向下。
记录轨迹：在机械臂运动过程中，记录末端执行器的位置。

代码示例

import sim
import numpy as np
import time# 连接到CoppeliaSim
def connect_to_simulator():sim.simxFinish(-1)  # 关闭所有先前的连接clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5)if clientID != -1:print('Connected to CoppeliaSim simulator')else:print('Failed to connect to the simulator')return clientID# 获取机械臂和末端执行器的句柄
def get_handles(clientID):_, ur5_joint_handles = sim.simxGetObjectGroupData(clientID, sim.sim_appobj_joint_type, 1, sim.simx_opmode_blocking)_, end_effector_handle = sim.simxGetObjectHandle(clientID, 'UR5_connection', sim.simx_opmode_blocking)return ur5_joint_handles, end_effector_handle# 设置关节角度
def set_joint_angles(clientID, joint_handles, joint_angles):for i in range(len(joint_handles)):sim.simxSetJointTargetPosition(clientID, joint_handles[i], joint_angles[i], sim.simx_opmode_oneshot)# 逆运动学求解
def inverse_kinematics(clientID, end_effector_handle, target_position):# 这里简单假设已经有逆运动学求解函数# 实际应用中需要使用更复杂的逆运动学算法_, current_position = sim.simxGetObjectPosition(clientID, end_effector_handle, -1, sim.simx_opmode_blocking)_, current_orientation = sim.simxGetObjectOrientation(clientID, end_effector_handle, -1, sim.simx_opmode_blocking)# 保持末端坐标系始终竖直向下target_orientation = [0, np.pi, 0]# 调用逆运动学求解函数# 这里只是示例，实际需要替换为真实的逆运动学求解代码joint_angles = [0, 0, 0, 0, 0, 0]return joint_angles# 规划路径并记录轨迹
def plan_path_and_record(clientID, joint_handles, end_effector_handle, start_point, end_point):trajectory = []num_steps = 10  # 路径步数for step in range(num_steps + 1):# 线性插值计算当前目标点current_point = [start_point[i] + (end_point[i] - start_point[i]) * step / num_steps for i in range(3)]# 逆运动学求解关节角度joint_angles = inverse_kinematics(clientID, end_effector_handle, current_point)# 设置关节角度set_joint_angles(clientID, joint_handles, joint_angles)time.sleep(0.1)  # 等待机械臂运动到指定位置# 获取末端执行器的当前位置_, current_position = sim.simxGetObjectPosition(clientID, end_effector_handle, -1, sim.simx_opmode_blocking)trajectory.append(current_position)return trajectory# 主函数
def main():clientID = connect_to_simulator()if clientID == -1:returnjoint_handles, end_effector_handle = get_handles(clientID)# 设置初始点和终点start_point = [0.5, 0.2, 0.2]end_point = [0.5, -0.2, 0.2]# 规划路径并记录轨迹trajectory = plan_path_and_record(clientID, joint_handles, end_effector_handle, start_point, end_point)# 打印轨迹print("Recorded trajectory:")for point in trajectory:print(point)# 关闭连接sim.simxFinish(clientID)if __name__ == "__main__":main()

代码说明

connect_to_simulator函数：用于连接到CoppeliaSim仿真环境。
get_handles函数：获取UR5机械臂关节和末端执行器的句柄。
set_joint_angles函数：设置机械臂关节的目标角度。
inverse_kinematics函数：根据目标位置和保持末端坐标系竖直向下的要求，求解机械臂的关节角度。这里只是一个示例，实际应用中需要使用更复杂的逆运动学算法。
plan_path_and_record函数：规划机械臂从初始点到终点的路径，并记录末端执行器的轨迹。
main函数：主函数，调用上述函数完成连接、设置初始点和终点、规划路径和记录轨迹的操作。