OK so I am looking at the HAL system to work out where we should integrate. "halcmd list all" is actually very like "rostopic list" (and other similar functions in ros), the two systems have quite similar architectures.
As far as I can tell, its the stepper motor that provides position feedback to the motion controller on pin "motor-pos-fb" (the motion controller also commands the stepper motor over motor-pos-cmd line). I tried manually disconnecting the signal and setting the values of motor positions using halcmd and the GUI did indeed jump the end effector, but then the system errored saying the joint was faulty. I assume thats a safety mechanism, I would like to know where its error tolerance is set on jerking the position feedback signal as we may need to lower that.
Optical feedback only comes at 30fps, it is a 6dof estimate of position + rotation. The motion controller needs signals alot faster that 30fps.
I have had a few ideas for integration. The issues to deal with are
1. latency on camera signals
2. slowness of camera signals
3. The existing motor fb signal will never get reset by the camera system, so could drift to ridiculous values
My best idea so far is when a camera frame comes in, we take the difference between its estimate of position and the motor pos fb value to create an error term. This error term can just be added to motor-pos-fb and fed into the motion controller. However, that probably won't work because of sharp changes in the error term (exacerbated by camera latency), so **we need a better way to smoothly alter the error term** (ideally without too much logic)*
Later we might want to linearly predict the expected camera signal based using latency, but not initially.
Tom
* my very first idea was a weighted sum between camera estimate and motor-pos-fb, but then I realised when the motor drifted alot the whole thing would be out of kilter all the time. The motor-pos-fb needs to be considered just a differential quantity rather than an absolute one (whereas camera-fb is our golden absolute feedback).