The status of gripper is not updating. For the FORAGE example, the robot never stops and does grabbing when the object was inside the gripper.
I also try the following in my program:
printf("Entering final approaching stage\n");
if (gp.inner_break_beam || (gp.outer_break_beam)
It prints the same breakbeam state of when the program starts.
Any help would be appreciated