Firmware Refactoring

[Lichtso]

In the last days we added lots of new features to the firmware, like jacobian force rendering and touch sensors. But the code quality dropped further and the overall structure was always somewhat chaotic. It would be good if you could refactor the firmware C code.

[JotaroS]

One thing which is quite tricky now is that the haptic actuation has separate two mode:

• Position based control mode
• Force rendering mode

which is defined by single bool variable and I hardcoded to get it work quickly, and I didn't like it. Position based control can already be replaced by force rendering using pseudo-inverse matrix technique, but i think it will be a good idea only if it really works and the code will be much cleaner

[mgjm]

I would like to have a look at it. But first I have a few questions @JotaroS, @Lichtso :

• Is there a reason why some stuff lives outside of the Panto class (e.g. encoder, destinationAngle, ...) and some stuff lives inside the Panto class (e.g. innerAngle, force, ...)
• What exactly do the following methods: inverseKinematics, forwardKinematics and applyForce? My understanding / guesses so far:
  • inverseKinematics: Calculates some kind of force related stuff out of the target position (only called once a target position is set)
  • forwardKinematics: Updates the current movement targets (called in the loop)
  • applyForce: Applies the force vector to the system (called in the loop by actuateMotors)
• What happens to the system when a position is not reachable (out of range or physically with the two arms not possible)
  • If there is some detection logic, is this logic also used in the force rendering mode
  • If not, what happens if a force vector is set and then the USB cable is disconnected

[JotaroS]

1. @Lichtso should have answer for that
2. For these 3 methods are robotics related terms :
   • Inverse Kinematics:: "Given a position of an end-effector, caluculate the angles of linkages". Note that this uses numerical approximation. Usually pantograph linkages can be caluculated by math, but math is sometimes difficult to stabilize the results for robots. Using numerical approximation works very well.
   • Forward Kinematics:: "Given a angles of (root) linkages, caluculate the position of an end-effector". Here FK uses theoretical values. At the same time, Jacobian matrix is caluculated. Note that the matrix is already transposed.
   • applyForce :: Calculates torques to apply to the motor given the force to apply on the end-effector using Jacobian given by FK. Called by loop when serial data contains force data.
3. Position control is caluculated by Inverse Kinematics, and IK prevents pantograph reaching out of opMaxDist on config.json given to each pantograph hardware. IK just returns values within such constraints. And yes, force rendering doesn't stop for now. We can implement the defensive function that stops force rendering (e.g. when no force rendering signal for 1 sec.)

[mgjm]

What is your opinion about moving more of the "complex" panto logic to the computer. My proposal:

• The dual panto calculates the angles (measured by the encoders) and sends the raw angles to the computer
• The dual panto gets from the computer a target angle, a strength and a duration value
  • based on the current angle, the target angle and the duration the dual panto can compute the exact expected angle for any given moment
  • the difference between the calculated / expected angle and the actual angle (measured by the encoders) can be used together with the strength value to determine how hard the dual panto works to reach the expected angle

This should be a minimal input / output definition that is needed to render all kind of movements. All angles could even be replaced by encoder steps and therefore eliminate floats on the Arduino completely. This would allow the movement loop to run much faster and to have a more direct physical feedback during movements.

The position updates could then be send less often as they are only needed for defining movement steps and not for direct physical feedback (e.g. 100 Hz). And this should still be fast enough for smooth and straight movements.

Walls for example could the be implemented by setting a position and a high strength, so that the dual panto allowed only a little movement. But for this to work a movement matrix would be needed to specify the strength depending on the two angle offsets and the direction (cw or ccw). To allow movement in all directions but not into the wall :D

[Lichtso]

We had that in the beginning, and are now trying to shift logic from the PC to the Arduino, not the other way around. The reason for that is:

1. There is enough computation power and memory on the Arduino.
2. The latency / delay should stay below 1ms, with haptic rendering at the PC we have at least 3ms.
3. We want to be hardware independent. The PC software should not have to care about the hardware version being connected. (Thus floats instead of encoder ticks).

Regarding your question about the member / global variables: It started with member vars but later the global config header file was introduced. Thus, they are mixed now. But if would be better to stick with one of them.

[mgjm]

Ok, I understand, but...

1. As far as I can see, currently the loop runs at 1kHz and I think that the more simple loop could run at at least 10kHz, thus giving a more responsive feedback.
2. I think that in my solution the Arduino is still capable to calculate the actual haptic feedback. The Arduino could calculate the angle difference and calculate out of this difference the amount of force to response with, without understanding the actual implications of the movement (i.e. without calculating x and y coordinates out of the angle) When the Arduino calculates the difference from the target angles the response could be in the area of 0.1ms (10kHz loop), so the physical response time is likely to become the bottleneck.
3. The software could load the device dependent values from the dual panto once a connection is established.

The in 2) and my last message described reactions of the Arduino should work, as long as the current position is still quite close to the expected position (i.e. when the Arduino reacts fast enough). Then the difference between simple angle offset calculations and the complex panto calculations should be negligible and result in a faster main loop and therefore in faster physical feedback.

[mgjm]

Some tests later... I made some simple tests and now I understand why my "simple solution" would not work. But after writing a speed test I have a new solution: I was able to achieve response times of 0.8 ms on my MacBook with the Arduino Due and the SerialPort library. I think this delay is small enough to let the pc calculate everything. The Arduino would then only send the values to the pc and gets the motor "commands" back. How and what did @Lichtso measured to get 3ms?

[Lichtso]

3ms is just an approximation:

• 1ms from Arduino to PC
• Some calculations, wait for next chance to send answer packet (1ms)
• 1ms from PC to Arduino

Could you show me how you achieved better performance using SerialPort than serial.c ? As far as I can tell SerialPort is also written in C/C++

[mgjm]

It is written in C++ and the poll logic (for non Windows systems) is in src/poller.cpp. The important part is that they are using libuv's poll functionality. Therefore the os directly signals the code whenever the readable state of the serial port changes (i.e. new data is available). This drastically reduces the delay for new data.

For windows there is some other logic. They do a blocking read on the serial port in a new thread. Therefore also on windows is virtually no delay.

And data can be send as soon as possible. So the only delay (that would add up to the current 0.8 ms) comes from the actual calculations. And those should be quite fast on a computer (compared to the Arduino).

[mgjm]

Benchmark results (now actually with 1 kHz, responses take longer when running at a lower rate):

Average response time:   243.1 µs
Responses under 1 ms:    52550
Responses over 1 ms:        30
Error rate:            0.00057

(Errors are responses that took longer than 1 ms, there where no actual transmission errors :D)

[JotaroS]

We should try libuv, since current example/wall/wall.js example we can fill bit of roughness that users feel the friction is intentionally rendered, but it is actually occurred by the latency (probably by arduino calculation).

We could also try moving Jacobian/FK/IK caluculation to the outside as you say (Actually Haply firmware did that), and minimize complex calculation in Arduino, but I guess such calculation is not a big deal for the MCU in due board.

How did you test the benchmark above? Did you try sending floating number to the arduino?

[mgjm]

I have updated the benchmark and now the Arduino sends a 24 byte package (6 32bit values) to the pc and the pc responds with a 12 byte package (6 16bit values). (New average response time 288.2 µs). So there are no float numbers sent, but the package size is realistic and therefore the type of values transmitted should not matter.

[JotaroS]

Great job, it should also be related to CPU consumption we talked a bit before. I put a new issue for the specific update.

[Lichtso]

Ok, we can improve the communication too, but that is a separate issue. This one here should be about the Firmware side alone.

[boeckhoff]

Whoever is in the process of refactoring should please include the bytes for button presses, see #90

[mgjm]

I started refactoring the firmware and the protocol. The refactored protocol is now extensible, so that dual pantos that support buttons could send this information.

Now I have two questions:

1. In the firmware the number of pantos is configurable, but this number is not visible outside of the Arduino (i.e. the javascript code does not know the number of pantos). Should this number be fixed to 2 or be configurable in the firmware?
2. And what are the "correct" names for the pantos / handles: Should they just be numbered 0 and 1, or called "meHandle" and "itHandle" or "upperHandle" and "lowerHandle" or "upperPanto" and "lowerPanto"?

What access by name could mean for the API (replace upperPanto with the correct name):

device.movePantoTo(device.upperPanto, new Vector(10, -20));
// or even better:
device.upperPanto.moveTo(new Vector(10, -20));

[Lichtso]

1. The Firmware sends the hash value of its configuration about every second. Thus, the PC software knows which configuration is in use (and also how many pantos).
2. In the Framework we should use ME and IT handle I suppose, but ask @oschneid For the Firmware however this is irrelevant and we just use the indices.

[oschneid]

Let's use ME/IT for terminology right now. Let's design this to be specific and generalize later. Eventually we could have an upper handle and lower handle, which are in Me mode or It mode....but that might confuse students.

I think splitting it into device.meHandle (or device.me maybe, but probably not) and device.itHandle makes sense.

[lukaswagner]

The firmware has changed completely since this issue was last discussed, this also included lots of refactoring.