机械臂相关问题

如何在不启用server的情况下运行所有任务

可以直接运行engine.py的文件。

如何单独执行机械臂的任务

1.初始化需要用到的类

arm = Arm()  # 初始化机械臂
camera = Camera()   # 初始化相机
camera.start()  # 开启相机
aruco = Aruco(0.034)    # 初始化要识别的aruco，参数为aruco的边长(单位为米)

2.开启处理相机的线程

worker = threading.Thread(target=produce_frames, args=[camera], daemon=True)

worker.start()

3.将任务所需的类传入以定义任务，并执行任务。例如，执行将试管从试管架转移到试管盘的任务：

task = Rack2TrayTask(arm=arm, camera=camera, aruco=aruco, row=1)

task.init()

task.execute()

其他任务的定义如下：

anarchic = Anarchic(arm, camera, pose_init_enable=False, camera_enable=True, inverse=1) #无序抓取识别和动作
task = DisorderlyGrabTask(arm, camera, aruco, anarchic) # 无序抓取任务

task = Rack2BoxTask1(arm=arm, camera=camera, aruco=aruco)   # 在无序抓取的底盘位置将试管架转移到箱子
task = Box2RackTask2(arm=arm, camera=camera, aruco=aruco)   # 在转移试管架到试管盘的底盘位置将试管架转移到桌面
task = Rack2BoxTask2(arm=arm, camera=camera, aruco=aruco)   # 在转移试管架到试管盘的底盘位置将试管架转移到箱子
task = Box2RackTask1(arm=arm, camera=camera, aruco=aruco)   # 在无序抓取的底盘位置将试管架转移到桌面

得到所有物品相对于机械臂基坐标系的位置

目前的计算位置的方法为了兼容以前写的旧代码，采用了迂回的方式进行计算，后续需要进行简化。

使相机坐标系的中心对准aruco的中心

（不需要真正对齐中心，只需要获取相机视野中心对齐aruco中心时机械臂末端所处的位置）

由于眼在手上，末端坐标系（end）与相机坐标系是固连的。

1.获取当前机械臂末端所处的位姿①，从而得到①位姿时，相机坐标系（cam）相对于机械臂基坐标系（base）的夹角（以逆时针为正）

cur_pose = arm.get_current_pose()
yaw_cam2base = 0.5357 + cur_pose.rz 
# 加0.5357是因为机械臂在初始状态（末端坐标系与机械臂基坐标系平行时）的rz（z方向旋转角度）是-0.5357弧度，加回来才等于0

2.相机坐标系的中心移到aruco的中心（未旋转对齐aruco，只是中心重合）

_ = find_aruco(camera, aruco, arm)  # 相机寻找aruco
pos_x = aruco.pos_x # 获取aruco在相机坐标系下的x方向的位置
pos_y = aruco.pos_y # 获取aruco在相机坐标系下的x方向的位置

R0 = np.array([[math.cos(yaw_cam2base), -math.sin(yaw_cam2base)]，[math.sin(yaw_cam2base), math.cos(yaw_cam2base)]])
T0 = np.array([[pos_y], [pos_x]]) # aruco相对于相机坐标系的位置向量

T1 = R0 @ T0 # 末端只需要移动T1就能使相机中心与aruco中心重合（未旋转对齐）

3.相机坐标系旋转对齐aruco

因为我们不能直接控制相机坐标系旋转，只能通过控制末端坐标系平移加旋转，从而使相机坐标系只是旋转。

yaw = aruco.yaw_angle   # aruco相对于相机坐标系的夹角（以顺时针为正）

R2 = np.array([[math.cos(-yaw), -math.sin(-yaw)], [math.sin(-yaw), math.cos(-yaw)]])    # aruco相对于相机坐标系的旋转矩阵（为了统一以逆时针为正，在前面加了个负号）

T2 = np.array([[0.08], [0.035]])    # 相机相对于机械臂末端的x和y方向的距离（绝对值）
T3 = R2 @ T2     # 相机坐标系旋转到对齐后的机械臂末端 相对于 未旋转的在aruco中心的相机坐标系的距离
T3 += -T2   # 末端坐标系 相对于 未旋转的在aruco中心的相机坐标系 需要平移的距离
T3 = R0 @ T3    # 末端坐标系 相对于 基坐标系 需要平移的位置

4.机械臂移动使相机视野对齐aruco的位姿为

cur_pose.px += T1[0] + T3[0]
cur_pose.py += T1[1] + T3[1]
cur_pose.rz += -yaw

根据物品相对于aruco的位置得到物品相对于机械臂基坐标系的位置

1.测量获取试管架第一个位置相对于aruco坐标系的x和y方向的偏置和旋转角度

2.让机械臂末端移动到试管架第一个位置

yaw_obj2base = 0.5357 + arm_cur_pose.rz + offset_angle # 物体相对于基坐标系的旋转角度
R0 = np.array([[math.cos(yaw_obj2base), -math.sin(yaw_obj2base)]，[math.sin(yaw_obj2base), math.cos(yaw_obj2base)]]) # 物体相对于基坐标系的旋转矩阵
T0 = np.array([offset_x - 0.035, -offset_y - 0.08]) # 物体相对于末端坐标系的位置
T1 = R0 @ T0 # 末端坐标系去到物体上方需要移动的距离
# 末端坐标系去到物体上方时的位姿
cur_pose.px += T1[0] + T3[0]
cur_pose.py += T1[1] + T3[1]
cur_pose.rz += -yaw_obj2base

3.让机械臂末端移动到试管架的第n个位置

每个试管孔之间的距离为2厘米，所以只需要修改offset的值

offset_x += 0.02 * (n - 1) * math.cos(offset_angle)
offset_y += 0.02 * (n - 1) * math.sin(offset_angle)