We have two grippers mounted at opposing 45° angles from the tool flange. When commanded to move the the same spot gripper 2 is slightly off from gripper 1. I want to calibrate the gripper 2 TCP (TCP_2) based on a fixed point set by the first gripper (TCP_1). Here is basically what I have so far.
set_tcp(p[0.14701,0.0,0.14701,0.0,0.7854,0.0])
move TCP_1 to calibration point
TCP_1_Point1=get_actual_tcp_pose()
set_tcp(p[-0.14701,0.0,0.14701,0.0,-0.7854,0.0])
move TCP_2 to calibration point
TCP_2_Point1=get_actual_tcp_pose()
TCP_Offset=pose_sub(TCP_1_Point1, TCP_2_Point1)
Where I am getting stuck is the TCP_Offset is in relation to the Base coordinate system but I need to translate it in relation to TCP_1_Point1 and apply it to the TCP_2 so it moves to the same position (For Example: if the Y was off +2mm the TCP would need a -2mm Y value to compensate).
Edit: I was doing my math wrong, I needed to do TCP_Offset=pose_trans(pose_inv(TCP_2_Point1), TCP_1_Point1).