- 为让TP总在爪子中心线，是通过两个操作。一是在state_distance阶段，会调用rotate_joint1_if_necessary，基本确保TP在爪子中心线。二是在state_claw前，会先进入state_align，在这状态会尽可能让TP对齐爪子中心线。
一、机械臂夹角
- 蓝色。相机夹角。相机测出的x、y。
- 绿色。机械臂夹角。用于求机械臂夹角的x、y。
- 橙色。机器人夹角。
在安装时，要确保机器人中心(base_footprint坐标系原点)、机械臂joint1坐标系原点、爪子中心，它们的y须在机器人y=0这条线上。相机原点y可以不在这条线，但不要偏移太多。让存在这个偏移，是出于这么个现实：相机坐标系原点不是在相机尺寸中心。对奥比中光相机DaBai DCL，同时接收RGB+深度时，相机原点是在RGB相机中心，而不是尺寸中心。安装时，为方便可能会把相机尺寸中心放在y=0这条线上。也就是说，机器人中心、机械臂joint1坐标系原点、爪子中心，相机中心（不是原点），它们的y会在机器人y=0这条线上。
参数RGB_diff_y指示相机原点到机器人y=0这条线的偏移。对DaBai DCL，这个值是1.1cm。
如果旋转的是机器人，要弥补的是机器人夹角(robot_theta)，图1中橙色。如果旋转的是机械臂，要弥补的是机械臂夹角(moveit_theta)，图1中绿色。对同一个TP，这两个夹角值不一样。计算这两个夹角时，y一样。对x，由于机械臂会放在底盘前半部，计算机器人夹角的x就要大于机械臂，所以同一时刻，机器人夹角要小于机械臂夹角。
机械臂原点取的是joint1坐标系，不是joint2。
实装时IMU时，会要求y尽可能在机器人y=0上，但不希望限定x。因而同一个TP，从它读出的yaw既不是机械臂夹角，也不是机器人夹角、相机夹角。因为x不确定，在操作机械臂时，全程不使用IMU的yaw。这就要求joint1转的角度和value得有一定的精准度。像转20度，误差不要超过1度。
1.1 计算机械臂夹角moveit_theta
const double RGB_diff_y = 0.011;
SDL_DPoint dcamera_origin{curr_fk.position.x + -1 * PRP.x, RGB_diff_y};
moveit_theta = atan2(dcamera_origin.y + TP.y, dcamera_origin.x + TP.x);“RGB_diff_y = 0.011”针对的是DaBai DCL相机。
“curr_fk.position.x”是图1中的fk.x。由正向运行学方程fk算出。
PRP.x是PRP这个点在相机world坐标系中的坐标，值是和TP同一帧拍到的。
“TP”是TP这个点在相机world坐标系中的坐标。
1.2 moveit_theta精准度
什么因素影响moveit_theta的精确度？
- 装配问题。joint1应该在y=0上，需保证爪子两瓣y=0对称。
- 计算moveit_theta的x、y。这里有一个fk.x，是个机械臂fk算出的值，如果机械臂误差较大，那这个值会不准。
对如何提高fk.x精准度？——通过两次两次拍到的TP、PRP值，算出它们之间距离TP_PRP_diff.x，并计算连续两次的距离差值TP_PRP_diff_diff.x，弥补fk.x，是否能提高精度。——这方案估计不行，主要两个原因。
- 在操和过程中，TP是可能会变的。
- 在两次计算TP过程中，joint1可能会发生了旋转。这在目标物位置不变的情况下，
由于fk.x有误差，这算出的moveit_theta会有误差。
1.3 让TP总在爪子中心线上
为让TP总在爪子中心线，是通过两个操作。一是在state_distance阶段，会调用rotate_joint1_if_necessary，基本确保TP在爪子中心线。二是在state_claw前，会先进入state_align，在这状态会尽可能让TP对齐爪子中心线。
bool rotate_joint1_if_necessary(const std::string& scene, const double moveit_theta, std::string& msg) @moveit_theta。机械臂夹角。图1中以机械臂原点为起点，绿色线指示的那个夹角。
在rotate_joint1_if_necessary，如果机械臂夹角在[-2, 2]，不做调整，返回false。否则要旋转joint1，目的是使得旋转后的moveit_theta是0。
在handle_distance执行后，只要此次是找到有效RP、TP，那意味着已调用过rotate_joint1_if_necessary，这“确保”了TP在爪子中心线上。由于state_ik是要在handle_distance返回true后才会执行，意味着在set_ik前，TP已在爪子中心线上。但“确保”加了引号，原因是在中心线，有误差。
- rotate_joint1_if_necessary只是限定在范围[-2, 2]。
- fk.x存在误差。
- joint1的精度。认为joint1关节value值的物理意义就是关节旋转弧度，要弥补moveit_theta，就是让joint1直接旋转moveit_theat弧度，如果设的value-diff不是对应world世界旋转过的弧度，那会产生误差。
对state_distance或state_ik，能有这个“确保”，基本也够了。但是，对按按钮，是否TP是否在爪子中心线，这会极大影响操作精度。为此在state_claw前，会先进入state_align，在这阶段会尽可能让TP对齐爪子中心线。
在state_claw，会算相机夹角。一旦相机夹角超过0.8度，就会试着旋转joint1。
在每次旋转joint1前，会记住此刻的相机夹角，记在min_abs_dcamera_theta_y0。一旦下一次相机夹角大于min_abs_dcamera_theta_y0了，就不再旋转joint1，同时结束state_claw，避免死锁。
二、ik_query.z
ik_query是用要于求逆解的目标位姿，类型geometry_msgs::Pose，但由于求出的ik逆解，只是得到xyz平移部分，可认为它就是个xyz三维坐标。逆解针对的是“ik_arm”这个关节组，包括从了joint2到joint4。因而ik_query.z是joint4坐标系原点在joint1坐标系下的z坐标。
图2中紫色横线是ik_query.z。ik_query来自机械臂的joint1坐标系。正如图2绿线所示，ik_query.z=0就是joint1坐标系原点所在的z平面。
<joint name="joint1" type="revolute"> <origin xyz="0.054476 0.00070272 0.156" rpy="0 0 0" /> <parent link="base_link" /> ... </joint>
0.156是joint1坐标系原点在base_link坐标系下的z坐标。ik_query.z=0不包括0.156。。
要注意，ik_query.z不是TP.z点所在的平面。以图2为例，在斜夹时，由于爪子较长，目标物才3cmx3cmx3cm，那ik_query.z要位在TP.z上方的square_xz(9.5cm)处，即TP.z还要往上9.5m的那地方。
顺便说下，图2中的TP来自相机坐标系。
三、求出ik逆解ik_result后，要操作机械臂到达ik_result，如何设置joint4值
PRP_joint_value = DEG2RAD(-10); // -10(rad:-0.174444)
ik逆解用的是ik_arm关节组，包括joint2、joint3、joint4关节。ik逆解，算出的只是join4关节要到达的(x, y, z)，至于旋转部分rpy是不确定的。joint4值不影响最后joint4要落在哪(x, y, z)，但影响rpy。
joint4的axis.y=1，也就说将按着数学课教的角度值和时针转动关系。顺时针转时，joint4值变小；逆时针转时，joint4值变大。对俯仰角(pitch)，朝向地面转的方向是逆时针。
到达ik_result后，joint4值要设为这多少。看下这值会产生哪些影响。
- joint4值过小，即过于朝向天。会使得相机支架碰到joint3关节。
- joint4值过大，即过于朝向地。当TP离地面较近时，在机械臂移动到目标点过程中，1）爪子可能会碰到地面。2）目标物体在物体2上，爪子可能会碰到物体2。
在设想使用场景时，总是假定目标物上面没东西了，为不碰到支撑目标物的地面或其它物体，那就让joint4尽可能朝向天，即一个尽可能小的值。但障于影响1，这个值不让能支架碰到joint3关节，目前取-10度。