操作机械臂

• RP(Reference Point)：参考点。存在爪子上的一个点，由机械臂提供商在图像中识别出来。y分量必须在机械臂中线上。
• PRP(Pitch Reference Point)：俯仰角参考点。位置上，x、z是PRP_joint关节原点，y等于参考点的y。为让爪子放在正确位置，一种方法是调爪子的俯仰角，调角度时就是通过这个关节。它的计算方法，RP坐标减去一个固定偏移得到，这个偏移等到RPR_offset + RPR_2offset。
• TP(Target Point)：目标点。在输入的彩色图像，识别小程序给出这个点。它指示机械臂要操作的目标物坐标。
• PRP_offset：有两个作用。1）计算PRP相机坐标。2）导找最优dcpitch。——moveit.rsp存储的dist是(RP-PRP_2offset.x)到末端的距离。
• PRP_2offset：只用于计算PRP坐标，这个坐标是在RPR_offset基础再加上，因而出现“2”字样。它不用于寻找最优dcpitch。

一、一次识别过程

夹取3cmx3cmx3cm方块实例。已启动深度相机、底盘（用于控制机械臂）、IMU（用于获取俯仰角pitch），负责识别方块的小程序是aplt.leagor.basic。

• 识别线程。深度相机的DoWork线程，任务是根据输入的彩色图像，告知TP在彩色图像中的像素坐标。
• 任务线程。求逆解，控制机械臂等，是些耗时操作，为不干扰主线程，让执行在任务线程。

1 (主线程)调用start_operate()，进入state_distance状态。深度相机把实时彩色图像交给小程序。

2 (识别线程)识别彩色图像，告知检测到一个目标点TP：color_points。彩色图像是二维图像，color_points是在该二维图像中的像素坐标。

3 (主线程)调用handle_distance()。

1. 在该彩色图像中找到参考点RP，得到参考点在彩色图像中像素坐标。
2. 深度相机在彩色深度同步功能，于是由之前彩色图像中的像素坐标，可在深度图像中查到对应深度，并计出相应RP、TP的相机坐标系下的笛卡尔坐标：ref_camera_xyz、target_camera_xyz。
3. 根据相机坐标系和世界坐标系位姿变换，得到RP、TP在世界坐标系下的笛卡尔坐标：ref_map_xyz、target_map_xyz。
4. 由IMU得到当前dcpitch，计算出在俯仰角值是dcpitch时的坐标：RP_、TP_。
5. 由moveit.rsp给出的爪子模型，调用RP_2_PRP，得到PRP_。
6. 计算接下机械臂要移动多少距离delta，因为y方向不会有变换，delta.y总是0，要计算的是delta.x、delta.z。接下移动是近距移动，在计算x时，除precise_gap_x外，还须加上blind_x_threshold。因为近距移动后，会有一次图像识别过程，只有precise_gap_x的话，爪子离目标物太近，会导致无法识别。
7. 由正向运行学方程得到当前fk，fk加上delta，就是接下求解的逆解query_fk。并进入state_ik状态。

4）(任务线程)在ik_arm组求逆解，得到组内关节状态joint_values。旋转各关节，让达到joint_values。

5）(主线程)发现已得到逆解，进入state_distance状态。深度相机再次把实时彩色图像交给小程序。转到步骤2。

6）(主线程)在步骤3中的3.6计算delta。接下要进行盲移，计算delta.x时只须考虑precise_gap_x，不再需要blind_x_threshold。同时会置near_move_finished=true，表示近距移动结束了。

7）(任务线程)和步骤4一样，求逆解，旋转各关节到joint_values。？但不再控制PRP关节必须角度？

8）(主线程)发现已得到逆解，进入state_claw状态。

9）(任务线程)由正向运动学方程得到当前fk，记为curr_fk，认为curr_fk减去near_moved_fk就是此次PRP移动了距离，得到“移动”后的PRP在世界坐标系坐标：PRP。根据PRP、爪子模型、TP，调用find_best_dcpitch，计算最佳dcpitch。find_best_dcpitch(...)得到的是深度相机俯仰角dcpitch，不是PRP_joint的旋转角度。

10）(任务线程)旋转PRP关节，让转出dcpitch。同时控制爪子，旋转出snapshot.value。

二、搜寻、对齐、可逆解

2.1 搜寻

搜寻指的是机器人导航到预设位置后，寻找目标物。搜寻可分为两步。

1. 在当前点找寻目标物。如果没有，机器人逆时针转45度，将这个点做为当前点。如果还是没有，继续转45度。直到转了一圈，如果都没有，此将操作失败。
2. 在当前点，再分两次。初始状态，搜5秒。如果没发现目标，相机向前伸出至少10厘米，称近距识别状态，再搜5秒。在当前点时，目标物或许离机器人较远，相机要近距识别状态才能发现。

让相机向前伸10厘米，可有两种方法。一是机器人向前移。二是机械臂向前移。用第一种方法，机器人移动有较大随机性，而且还得退回来。这里选择使用移动机械臂。于是机械臂存在两个和识别相关状态，初始识别状态(state_recognize)和近距识别状态 (state_recognize_near)。

在搜寻过程，机器人会发生的移动是线速度为零的原地转。

2.2 对齐

对齐指的是要让目标点尽量在爪子中心线上。想象下夹取苹果，苹果中心点要在爪子中心线上。当然，实际很难做到在中心线，为此只能是尽量。对齐有两种方案，一是旋转机器人，二是旋转机械臂，具体是当中joint1关节。

方案之旋转机器人

旋转机器人时，虽然会发给一个线速度为0的速度，但转后，不能保证机器人旋转后还在原坐标。

转了几次，快到目标角度时，难免要用到小速度。根据经验，最终中心线和目标点夹角不让超过6度，要到达6度内误差，对底盘速度控制还是有点要求的。

这里暂不用横移，即只有y不是0的速度。考虑到一些机器人并没有很好做到横移，不想只是因为这里，而要求机器人有很好横移性能。

方案之旋转机械臂

相比旋转机器人，旋转机械臂能容易到达满足对齐。不过它有个缺陷，当距离不满足时，像不在可逆角薄围，要移动dist米。通过dist*cos(yaw)是可以把要移动的距离算出来，可这样，角度就不满足了。

在对齐过程，机器人会发生的移动是线速度为零的原地转。

2.3 混合搜寻、对齐、前后

前后指的径直向前或径直向后。而不论搜寻还是对齐，都是拐弯。实际情况下，搜索、对齐不是可以独立分开的两个阶段，而且还会和前后混合，即机器人一会儿前后，一会儿拐弯，不断夹杂。让看示例。

1. 在初始识别状态，5秒内没发现目标物，进入近距识别状态。发现目标物，进入对齐。
2. 完成对齐，进入求逆解。发现距离不在可逆解范围，需后退。
3. 后退后，发现没了目标物，进入搜寻。即使有目标物，角度不满足，进入对齐。

一旦不在逆解范围，就会再次搜寻、对齐，于是会看到机器人移动不断出现后退、拐弯。

机器人发生前后移动后，机械臂会强制回到初始识别状态。原因是近距识别状态，可能发生丢失目标物，那还是得回到初始状态。于是会出现从近距状态回到初始状态，使得判断机器人是否发生过前后移动更复杂了。

1）如果是action_navigation_near才找到的，移动距离通过fk差。2）如果5秒内仍旧没有收到可识别帧，认为此个角度失败，换下个角度。

对前后移动，有时可能要求只是小距离，像1cm。机器人很难做不到只移动这么点距离。有人会想到这么个方法：先后退5cm，再前进6cm，从而前面1cm。没采用这，这有两上原因。1）每次后退、前进，并不能保证会有操作。2）即使有操作，误差有时较大，像超过3cm，在这时还不如直接跑个3cm。

2.4 判断机器人是否发生过前后移动

发送命令前后移动的速度，2.5秒后，需要判断机器人是否有移动，没有，要再次发送速度。原因是发送速度后，机器人可能不动。就没动过，后面ik肯定失败，还是会回来继续发送速度，徒增时间。

计算出TP和PRP之间x距离(TP_PRP_diff_x)，如果前后两次距离差值超过1cm，认为移动过，否则没有。这里有个问题，机器人发现需要移动时，机械臂在哪个状态是未知的，可能初始状态，可能近距状态，可能其它位置。而在判断是否有移动，是要恢复到初始状态。即计算TP_PRP_diff_x是在任意状态，而判断是在初始状态，如何在任意状态计算出初始状态时偏移(initial_fk_TP_RP_diff_x)？——要能发现目标物，至少有一次或是在初始状态、或是在近距状态，记住当时TP_PRP_diff_x作为initial_fk_TP_RP_diff_x。当然，如果在近距状态，须要加上初始、近距两状态下的PRP偏移。以下是一次在近距状态时，IK失败时流程。

1. (state_distance)在handle_distance，计算初始状态时initial_fk_TP_RP_diff_x。但现在近距状态，要在当前TP_RPP_diff_x上，加两个状态的PRP偏移。
2. (state_ik)在近距状态，IK失败。initial_fk_TP_RP_diff_x赋值给operate_.TP_PRP_diff_x。机械臂.5恢复到初始状态。发布后退速度。
3. (state_distance)2.5秒后。计算此时TP_PRP_diff_x，判断和operate_.TP_PRP_diff_x差是否超过1cm。

如何计算两个状态时的PRP偏移。目前用的是正向求解(fk)两状态时fk_xyz，然后得到x分量差值fk_diff_x，这个就做为PRP偏移。也就是说，urdf写着多少，那就是多少。如果想验证这值有多少误差，可进入lancher的“深度相机”，“Start”——“近距识别校准”。走完流程后，会弹出图2对结果。

核验靠的是用深度相机得到的坐标计算PRP偏移。具体来说，核验时保证机器人和TP不变，只动机械臂，进行两次计算PRP和TP之间的x，方法“TP.x - PRP.x”。第一次在初始状态，第二次在近距状态，然后得到这两次x的差，即图2中PRP_diff。如果精准的话，这个PRP_diff.x和fk_diff_x的误差error_x应该很小。

对判断方法，没用机器人在地图中的坐标变化。这种方法必须开着雷达、cartographer。

三、横抓

代码中grasp_horizontal表示用横抓。因为相机支架，导致横抓对距离要求较严格，目前不使用。

上图情况，要抓取顶上黄色积木。采用横抓，即最后dcpitch是0，那意味着要求joint4逆时针转。但是，因为相机支架被机械臂档了，实际已不可能再改变dcpitch。

四、zx查找表

zx查找表指示了对这个机械臂，笛卡儿坐标系中z、x坐标和各关节旋转值的对应关系。命名上z放在前面，是因为在使用时，z是预知的，x则须要某种手段测出来，最终是机器人要通过不断移动，满足距离x。以图1为例，预先知道z是25cm。

建立查找表时，生成每个条目则是从到。这包括两部分，一是正向运动学求解，二是误差。

查找表条目数？——针对每个z，或许只要一条就行，因而z的粒度决定表的长度。