OpenServo.com

[mpthompson]

I've been exchanging emails and IMs with Barry for the last few weeks discussing how to implement sophisticated motion control for the OpenServo using cubic curves. Based on experience I have gained in other areas, I'm proposing that we implement Hermite Curve Interpolation for motion control within the OpenServo. Hermite curves are a cubic curve closely related to Bezier curves, but rather than for control points in Bezier curves the middle control points are replaced with tangent vectors. The benefits of using a Hermite curve for motion control of the OpenServo is that they can be calculated very quickly and they are used to control animation in some of the most popular 3D animation packages such as Maya and 3D Studio. This would allow us to export animation curve data directly from such tools to control servo motion. More information regarding Hermite curves can be found here.

Basically it will work as follows. The servo controller will pass down keypoints for curves that describe the motion control (a time delta, position and velocity) which will be queued in a buffer on the OpenServo. The OpenServo will play out curves in 10ms increments. As long as the controller replenishes the queue the servo will maintain very smooth motion following the paths described by the curves. If the queue is ever exhausted the servo will hold the last position.

I have an implementation nearly complete for buffering curve data and then playing the curve data out on the OpenServo. I wanted to open this discussion up to get other peoples thoughts as well as Barry and I are hammering out the details.

-Mike

[ginge]

Just to give you an idea of what Mike is talking about, I have done a quick snapshop of the program I wrote to test some cubic hermite curves.


Click for bigger

Grid X is time in ms and Y is servo position.

Each curve section is sent to the servo until the buffer is full. Once the buffer is emptied sufficiently, the next segment of the curve is sent.

As you can see, you can build very complex curves without much thought.
With this functionality you can program the servo to gently sweep into position and also follow complex movement patterns.

When I have finished this demo prog I will port to Win32 if anyone is interested. I wrote it in QT so porting should be easy enough.

NOTE:I won't get the full resolution of the above curves from the servo, but it will scale properly to a realistic timespam. I used this 2.5s timespan as an illustration only.

Barry
_________________
http://www.headfuzz.co.uk/
http://www.robotfuzz.co.uk/

[mpthompson]

Terrific picture Barry. Illustrates the feature perfectly.

I'm thinking that each keypoint for the curve will consist of the following data:

2 bytes - unsigned 16bit time delta from previous keypoint
2 bytes - unsigned 16bit position
2 bytes - signed fixed point 6:10 bit in-tangent
2 bytes - signed fixed point 6:10 bit out-tangent
--------
8 bytes total

We are scrunching the tangent values a bit to keep them in 2 bytes, but tangent value range will be -32.0 to +32.0 which I believe is beyond the velocity the OpenServo can achieve in 1 millisecond. If these prove to be insufficient after testing we can expand the tangent values to 3 bytes (24 bits) each.

-Mike

[jamma]

This looks great, guys, and seems a natural fit for robot animation (the reason many of us are following this project). A silly question for you, though: Does this curve approach make the integration of gyroscopic (or other sensor) feedback more difficult to implement? The big trend in RoboOne is to use gyroscopes/accelerometers/kinesthetic sensors/etc to modulate planned movements to keep robots from tipping over. My pea brain can't figure out whether this approach would make such modulation more difficult.

[ginge]

Mike:-

It looks good. The tangent precision will be fine I am sure.

Jamma:-
I don't see why it would make it too difficult. I don't really have a grip on what you biped folks are using for algorithms, maybe you could enlighten me or point me to some resources?
I don't see a reason why with a little manipulation you couldn't send over a queue of modulated curves. You don't have to send the curve keypoints a long time before a move is in progress in this scheme, you can recalculate your curve parameters in response to the external balance issues. I will revise this statement once I know what I am talking about

Barry
_________________
http://www.headfuzz.co.uk/
http://www.robotfuzz.co.uk/

[mpthompson]

Jamma, the nice thing about Hermite curves is that it's trivial to cut them up to smaller sizes and blend them with other curves. I'm working on related issues myself in this area (not with the OpenServo unfortunately). Rather than sending the OpenServo a sequences of curves lasting many seconds in length a controller for a balancing robot would likely segment the curves into shorter durations (probably no longer than several hundred milliseconds), modify the curve segments based on gyro feedback and then send the modified segments to the curve queue. The advantage of this approach is that the servos are still following a prescribed path, but the path is altered just enough to achieve dynamic balance. This also allows the controller to deal with longer spans of time (to reduce work load) while the OpenServo concerns itself with following the curves smoothly on its own 10ms processing cycle.

A concern I have with the OpenServo is that I2C does put some additional delay between gyro feedback and fresh curve data (particularly with a large number of servos), but I'm not sure if this concern is valid at faster I2C clock frequencies. The trick is to not have too much of a delay between gyro feedback and what the servo is doing, but not doing so much work that you exceed the controllers computational capacity.

I would love to hear other peoples thoughts on this issue. As I mentioned, I'm working in the area of biped balance and have a general idea of what I want to do, but haven't yet hammered out the details.

-Mike

[mpthompson]

I started to check into the OpenServo CVS tree the source code needed to implement curve based motion control. These new files include motion.c/h and curve.c/h which include the bulk of the implementation.

The functionality is not yet connected to the registers to add curves or a modification of the PID algorithm to factor in both position and velocity in the PWM calculation. I hope to make these changes in the coming week or so.

I need to open up more register memory to make room for the window of registers that will store data for a keypoint. Shifting registers around may impact existing OpenServos so we'll need to do this carefully.

-Mike

[ginge]

Hi Mike,

Regarding the register layout, I successfully used registers 0x38 onwards for the testing I was doing. It's a nice open register space, and it doesn't overlap with Stefen's motion estimator registers.

I arranged it with 0x38+0x39 as the command registers, and 0x40 onwards for the window.

Can we use this area for the window without having to move any registers?

I also think that it might be prudent to make the ADC sample time a defined value, so that if any changes happen to CRVALUE in adc.c it won't affect too much of the code base. This affects motion.c (line 267). It's a trivial fix, and I will commit to CVS if you agree.

Barry
_________________
http://www.headfuzz.co.uk/
http://www.robotfuzz.co.uk/

[mpthompson]

Barry,

I agree the motion increment time should be tied to the ADC sample time as they are intimately connected. Is your change a global define that sets the ADC sample time at 10 ms? Go ahead and check your change it and I'll get back to you if I have and comments or suggestions.

For the registers, I was thinking of placing them a little differently to preserve SRAM space on the AVR. Use 0x30 - 0x37 as the window in the following manner:

0x30 & 0x31 - unsigned 16bit time delta from previous keypoint
0x32 & 0x33 - unsigned 16bit position
0x34 & 0x35 - signed fixed point 6:10 bit in-tangent
0x36 & 0x37 - signed fixed point 6:10 bit out-tangent

We would shift Stephan's registers for the estimator up to 0x38 thru 0x3F or make them defined constants so they aren't in the register space. (BTW, I need to catch up with Stephan on where things are at with the state estimator and regulator for control of the servo).

Then use TWI command bytes for the following curve operations:

0x91 - TWI_CMD_CURVE_ENABLE
0x92 - TWI_CMD_CURVE_DISABLE
0x93 - TWI_CMD_CURVE_RESET
0x94 - TWI_CMD_CURVE_APPEND

To program a curve into the buffer you would basically send an I2C packet to write registers 0x30 thru 0x37 with the keypoint data followed by the 0x94 curve append command which would shift the values into the buffer. Once curve data is appended the OpenServo would reset registers 0x30 to 0x37 to zero to minimize the next write if the next keypoint has velocity tangents at zero.

Registers 0x2E and 0x2F would be the curve status registers. The only status I can think of right now is buffer slots available for writing keypoint data -- basically many more keypoints can be pushed to the OpenServo before buffer overflow. We could include time remaining in the curves if we think that would be useful, but that would occupy at least two register bytes.

What were your thoughts considering the command registers? I'm I replacing the need for those registers with explicit TWI commands?

-Mike

[ginge]

yes, it would be a global define. I will take at look at integrating the ADC sample time into the motion function early this week.

The register layour makes sense. It's a good idea to allow for zero velocity with a register clear.

[mpthompson]

Barry,

I was trying to determinte the following from your previous post:

[ginge]

Mike, I see what you mean.

What I did was to use these registers as a 16 bit command register. Rather than using 0x80-0x99 I used these 16 bit registers to append the curves and initiate the movement. I guess it's reinventing the wheel somewhat, but does give some flexibility down the line with regard to the amount of commands the OpenServo can accommodate.

I doubt this useage is needed.

Barry
_________________
http://www.headfuzz.co.uk/
http://www.robotfuzz.co.uk/

[mpthompson]

I checked in the complete implementation of the curve based motion control of the OpenServo. Also, the code includes a revised PID algorithm that factors in velocity as well as implements a digital filter on the ADC position to help eliminate twitchiness caused by noise.

The final register layout is:

0x18 & 0x19 - unsigned 16bit time delta from previous keypoint
0x1A & 0x1B - unsigned 16bit position
0x1C & 0x1D - signed fixed point 6:10 bit in-tangent
0x1E & 0x1F - signed fixed point 6:10 bit out-tangent

The command values are:

0x91 TWI_CMD_CURVE_MOTION_ENABLE
0x92 TWI_CMD_CURVE_MOTION_DISABLE
0x93 TWI_CMD_CURVE_MOTION_RESET
0x94 TWI_CMD_CURVE_MOTION_APPEND

I look forward to getting a report from Barry how this worked out with his robot arm. Perhaps we can get a video?

-Mike

[ginge]

Nice work.

I will see if I can get the code flashed in and verified.
It's going to take a little more work to my Robot Arm codebase to get the curve functionaliy fully implemented. I'm going to need to do a lot of calculations to generate the optimal cure for the motion. Oh well.

Barry
_________________
http://www.headfuzz.co.uk/
http://www.robotfuzz.co.uk/

[ginge]

Hi all,

I have verified that the code is doing what I expect it to do, and I have updated my control application (see picture at top of thread) to calculate OpenServo values for the specified curves.

If anyone is interested I have detailed this on the wiki at the Utilities page
and a download link for Windows (win32)

Barry
_________________
http://www.headfuzz.co.uk/
http://www.robotfuzz.co.uk/