Changes in gripToken()

[shrine79]

@alecive If possible, will you bring back and test the fine calibration of arm based on hand camera? It would help in a dynamic environment. For example the table moves slightly over time or the tokens are needed to be cleaned from the chessboard by Baxter. Or do you have any special reason not to use the fine calibration? I've had my algorithm tested against static camera image but not yet merged into my code. Your experience helps.

Also, I have problem with the current way to sink end effector into tokens. The arm works fine with default position of token pool. However in my case some tokens are located closer to Baxter on the table. The current code has no problem to hover end effector over the tokens, but arm slides slightly towards the body of Baxter while reaching down onto the tokens, even if the code tell Baxter to lower end effector straight down.

My idea is that the computeIK() always allows a small amount of positional error in each invocation and always chooses the trajectory with least effort in joint rotation. The current gripToken() calls computeIK many times to lower the end effector, so the error accumulates and don't cancel each other. The arm drift would be noticeable if the lowering step is short enough.

I haven't found a perfect solution to work with the IR sensor.

[alecive]

These seems different issues to me. Can you please split them in atomic issue such taht I can have a better understanding? While doing so, please also specify what do you mean with:

1. If possible, will you bring back and test the fine calibration of arm based on hand camera?
2. Also, I have problem with the current way to sink end effector into tokens.
3. I haven't found a perfect solution to work with the IR sensor.

Please provide videos where needed :)

[shrine79]

1. In the legacy mode, the gripToken uses camera on the Baxter hand to make fine adjustment to the arm in x-y plane while lowering the gripper onto token. I think that's a good idea but don't why it's deprecated. Possible to bring it back?

2. The current gripToken() try to lower the gripper along z axle in a loop until IR sensor says too close. This works fine when tokens are placed in the "comfortable zone" of the arm. However when the tokens are placed closer to the robot, the gripper drifts to the robot body and the tip of gripper points to the body slightly as it goes down.

3. The IR sensor need to be polled frequent enough to find the right distance to stop, but the error would accumulate if computeIK() get called too many times over a short distance. I haven't made the code to poll IR sensor while just call to computeIK() once.

The point 1 is separate to point 2 and 3, but they all aim at improving the pick precision. I'd like to provide videos but I don't have access to Baxter in winter break (southern hemisphere).

[alecive]

1. I agree that it is a good idea. But right now it is not the default because it was not working 100% of the time. It was working 85% of the time, some time missing the token. I don't have the resources (either time or people) right now to investigate on that, so I preferred to remove the behavior altogether right now.
2. I need more explaination for this: "However when the tokens are placed closer to the robot, the gripper drifts to the robot body and the tip of gripper points to the body slightly as it goes down." I need a drawing or a picture or something that lets me understand why the tip of the gripper "points to the body as it goes down". Multiple reasons might result in this.
3. "The IR sensor need to be polled frequent enough to find the right distance to stop, but the error would accumulate if computeIK() get called too many times over a short distance." I don't believe this is true. Why do you say this? IR sensor needs to be called as frequent as needed in order to compute the current distance of the end effector, there is no accumulation.

[shrine79]

1. I agree the adjustment based on hand camera is not 100% reliable. It suffers from change in lighting conditions. In our case, light could cause a specular on the token as it's made from acrylic. The hovering height and the initial position of the token in picture would affect it as well. Based on my experiment, may I suggest to use cv::moments() function to calculate the centre of mass of the "O/X" on the token. The current code looking for the left-est and top-est pixels to determine the outside rectangle of the token, which is prone to noise in the image processing.

2. I had a illustration here. The left scenario shows the "comfortable zone" for baxter IK, where less joint efforts are needed to lower the gripper in each step. The right scenario shows the problem when token is too close to Baxter. It's still reachable, but it involves more effort to move joints. Our IK is lazy and would think the goal is reached as long as the error is less than certain fixed amount. In this case, pointing the gripper towards robot body represents less joint effort and IK always prefer it. The error is negligible if the moving distance is long enough in each step. However considering calling to computeIK() 100 times per second and just lower it a few millimetres each time, the error accumulation may be considerable. I have no problem when I lower Baxter arm straight down in single step.
   

3. This one is related to point 2. The error accumulation is not about IR, but the IK. I understand it's the right way to use IR but suffers from error in IK.

[alecive]

1. Using moments will not solve the issue. The issue is in moving from the detection of the object in 2D to its estimation in 3D, which is a much more difficult problem. But feel free to work on it if you have time!
2. Ok now I understood. But the issue is different. There are two options when too close: i) IK does not find a solution, and just fails (because a solution does not exist) ii) if IK solves the solution, the actual positions sent to the joints are different because the collision avoidance system changes them. IK always tries to solve the task for a vertical pose, so if at time t=t0 the computed pose is slightly different, at time t=t1 this error should not be propagated. What you see there is not an issue with the IK, but probably an issue with the collision avoidance (or collision detection) system.