This very insightful forum post (http://forums.tribotix.info/view_topic.php?id=76&forum_id=1&highlight=curve) describes how the CM5 c library (Bioloid Firmware) calculates and generates S-Curves . Has anyone written their own S-Curve generation? Is it worthwhile doing so in reality does it make a massive difference?

Rob

Rob

I've implemented S-Curves with constant and sinus wave acceleration profiles (in Bioloid Control):

***Images copied to RoboSavvy.com/Builders/cosa/ because imageshack has been known to delete images after some time***

Edit: new graphs:

no S-Curves

Sine S-Curves

Comparison

I'm doing the interpolation (splines, bezier, ..) on the pc side and send one interpolated trajectory point every 40ms to the uC program. The uC program takes the point and calculates ptp movement with linear interpolation or one of the two s-curve profiles.

There's no big difference between the two s-curves and simple linear interpolation.

Code (dirty):

There's a test program at the bottom of the page.

Parameters:

readPause = 2; //pause between keyframes in milliseconds

totalipo = 20; // total number of keyframes

time = 25; // acceleration end time 25% * totaltime

***Images copied to RoboSavvy.com/Builders/cosa/ because imageshack has been known to delete images after some time***

Edit: new graphs:

no S-Curves

Sine S-Curves

Comparison

I'm doing the interpolation (splines, bezier, ..) on the pc side and send one interpolated trajectory point every 40ms to the uC program. The uC program takes the point and calculates ptp movement with linear interpolation or one of the two s-curve profiles.

There's no big difference between the two s-curves and simple linear interpolation.

Code (dirty):

There's a test program at the bottom of the page.

Parameters:

readPause = 2; //pause between keyframes in milliseconds

totalipo = 20; // total number of keyframes

time = 25; // acceleration end time 25% * totaltime

- Code: Select all
`// ptp movement (sinus wave acceleration)`

void writePositionDataPTPPrepareSinus(unsigned long ipoTime, unsigned long totalTime, unsigned long time, unsigned long max, unsigned long param, float* readBuffer, float* writeBuffer, float* tbs, float* tes, int* sgns, float* vmaxs, float* bmaxs)

{

int i;

float current, target;

float MAXS = max / 10.0;

float TIME = totalTime;

float BTIME = (float)time / 100.0;

float VMAX = (MAXS / ((1.0 - BTIME) * TIME)) ;

float BMAX = (2.0 * VMAX / (BTIME * TIME));

float vmax, bmax, se;

long int te, tb, tv, sgn;

int ipo = ipoTime;

// prepare write buffer -> unpack data from serial and store bytewise in buffer

for (i=0;i<AX12_COUNT;i++)

{

target = writeBuffer[i];

current = readBuffer[i];

vmax = VMAX;

bmax = BMAX;

se = (target - current);

if (se < 0)

{

sgn = -1;

se = -se;

}

else sgn = 1;

if (vmax*vmax > bmax * se)

{

vmax = sqrt(bmax * se);;

tb = ((long int)( 2.0 * vmax / (bmax * ipo))) * ipo;

tv = TIME - tb;

} else

{

tb = ((long int)( 2.0 * vmax / (bmax * ipo) + 0.5)) * ipo;

tv = TIME - tb;

}

if (tb == 0)

tb = 1;

if (tv == 0)

tv = 1;

te = tv + tb;

vmax = se / (float)tv;

bmax = 2.0 * vmax / (float)tb;

tes[i] = te;

sgns[i] = sgn;

tbs[i] = tb;

vmaxs[i] = vmax;

bmaxs[i] = bmax;

}

}

// ptp movement (sinus wave acceleration)

void writePositionDataPTPSinus(FILE* writeFile, unsigned long readPause, float* readBuffer,float* writeBuffer, float pos, float* tb, float* te, int* sgn, float* vmax, float* bmax)

{

int i;

long int current;

float se, t, tv;

// prepare write buffer -> unpack data from serial and store bytewise in buffer

for (i=0;i<AX12_COUNT;i++)

{

current = readBuffer[i];

t = pos * te[i];

tv = te[i] - tb[i];

if (t <= tb[i])

se = bmax[i] * (0.25 * t * t + (tb[i] * tb[i] / (8.0 * M_PI * M_PI)) * (cos(2.0*M_PI*t/tb[i])-1.0));

else if (t <= te[i] - tb[i])

se = vmax[i] * (t - 0.5 * tb[i]);

else

se = 0.5 * bmax[i] * (te[i] * (t + tb[i]) - 0.5*(t*t + te[i]*te[i] + 2.0 * tb[i]*tb[i]) + (tb[i]*tb[i]/(4.0 * M_PI * M_PI))*(1.0 - cos((2.0*M_PI/tb[i])*(t - tv))));

current += sgn[i] * (long int) (se);

if (current > 1023)

current = 1023;

else if (current < 0)

current = 0;

if (i==0)

{

char buffer[255];

sprintf(buffer, "%d\n", current);

fputs(buffer, writeFile);

}

}

}

// ptp movement (constant acceleration)

void writePositionDataPTPPrepare(unsigned long ipoTime, unsigned long totalTime, unsigned long time, unsigned long max, unsigned long param, float* readBuffer, float* writeBuffer, float* tbs, float* tes, int* sgns, float* vmaxs, float* bmaxs)

{

// conversion variables

int i;

float current, target;

float MAXS = max / 10.0;

float TIME = totalTime;

float BTIME = (float)time / 100.0;

float VMAX = (MAXS / ((1.0 - BTIME) * TIME)) ;

float BMAX = (VMAX / (BTIME * TIME));

printf("VMAX: %g BMAX: %g\n", VMAX, BMAX);

float vmax, bmax, se;

long int te, tb, tv, sgn;

int ipo = ipoTime;

// prepare write buffer -> unpack data from serial and store bytewise in buffer

for (i=0;i<AX12_COUNT;i++)

{

target = writeBuffer[i];

current = readBuffer[i];

vmax = VMAX;

bmax = BMAX;

se = (target - current);

if (se < 0)

{

sgn = -1;

se = -se;

}

else sgn = 1;

if (vmax*vmax > bmax * se)

{

if (i==0)

printf("too big\n");

vmax = sqrt(bmax * se);

tb = ((long int)( vmax / (bmax * ipo))) * ipo;

tv = TIME - tb;

} else

{

tb = ((long int)( vmax / (bmax * ipo) + 0.5)) * ipo;

tv = TIME - tb;

}

if (tb == 0)

tb = 1;

if (tv == 0)

tv = 1;

te = tv + tb;

vmax = se / (float)tv;

bmax = vmax / (float)tb;

tes[i] = te;

sgns[i] = sgn;

tbs[i] = tb;

vmaxs[i] = vmax;

bmaxs[i] = bmax;

}

switch (param)

{

case 1:

// synchronous ptp

// get max total time

long int temax = 0;

for (i=0; i<AX12_COUNT; i++)

if (tes[i] > temax)

temax = tes[i];

// recalculate max velocity and acceleration times

for (i=0; i<AX12_COUNT; i++)

{

target = writeBuffer[i];

current = readBuffer[i];

se = (target - current);

if (se < 0)

{

se = -se;

}

bmax = bmaxs[i];

vmax = bmax * temax;

vmax = 0.5 * vmax - sqrt(0.25*vmax*vmax - se * bmax);

tv = ((long int)( se / (vmax * ipo) + 0.5)) * ipo;

tb = temax - tv;

vmax = se / (float)tv;

bmax = vmax / (float)tb;

tes[i] = temax;

tbs[i] = tb;

vmaxs[i] = vmax;

bmaxs[i] = bmax;

}

break;

}

}

// ptp movement (constant acceleration)

void writePositionDataPTP(FILE* writeFile, unsigned long readPause, float* readBuffer,float* writeBuffer, float pos, float* tb, float* te, int* sgn, float* vmax, float* bmax)

{

int i;

long int current;

float se, t;

// prepare write buffer -> unpack data from serial and store bytewise in buffer

for (i=0;i<AX12_COUNT;i++)

{

current = readBuffer[i];

t = pos * te[i];

if (t <= tb[i])

se = 0.5 * bmax[i] * t * t;

else if (t <= te[i] - tb[i])

se = (vmax[i] * t - 0.5 * vmax[i] * vmax[i] / bmax[i]);

else

se = (vmax[i] * (te[i] - tb[i]) - 0.5 * bmax[i] * (te[i] - t) * (te[i] - t));

current += sgn[i] * (long int) (se);

if (current > 1023)

current = 1023;

else if (current < 0)

current = 0;

if (i==0)

{

char buffer[255];

sprintf(buffer, "%d\n", current);

fputs(buffer, writeFile);

}

}

}

void CTest::run(float* args, int argcount)

{

float current[AX12_COUNT];

float target[AX12_COUNT];

current[0] = 100;

target[0] = 400;

// important

unsigned long readPause = 2; //pause between keyframes in milliseconds

int totalipo = 20; // total number of keyframes

unsigned long time = 25; // acceleration end time 25% * totaltime

unsigned long max = 100; //

unsigned long param = 0;

float tbs[AX12_COUNT];

float tes[AX12_COUNT];

int sgns[AX12_COUNT];

float vmaxs[AX12_COUNT];

float bmaxs[AX12_COUNT];

writePositionDataPTPPrepare(readPause, totalipo * readPause, time, max, param, current, target, tbs, tes, sgns, vmaxs, bmaxs);

FILE *file;

file = fopen("data.txt", "w+");

float pos;

for (int i=0; i<=totalipo; i++)

writePositionDataPTP(file, readPause, current, target, (float)i/(float)totalipo, tbs, tes, sgns, vmaxs, bmaxs);

fclose(file);

}

Last edited by cosa on Fri Apr 25, 2008 2:27 pm, edited 1 time in total.

The "no S-Curve" graph is exactly the same as the one you've posted on the tribotix forums. The sine wave s-curve should have (theoretically) no jerk at all since the acceleration curve is differentiable (if I remember it correctly). I don't have a picture right now, but you can think of it at as sine approximation of the rectangular shaped acceleration profile of the "no S-Curve".

I agree that your no s-curve looks similar to the one I posted. A trapezoidal profile is a similar with respect to position but I assure you my plots do not come from a trapezoidal function. The sine function you are plotting looks a little asymmetric but I have not run the numbers to see that it really is. To minimize jerk it should be symmetric on either side of the increasing to decreasing.

I hope this helps

Hopefully, everything should work as intended now.

Back to topic:

I thought of deacceleration as braking. Therefore I call the phase in which the acceleration is positive acceleration phase and the phase with negative acceleration deacceleration phase. Sorry for that.

The acceleration profile of the sinoidal type looks like this (top-left graph):

sinoidal

The function is of type sin^2. Through 2x integration you'll arrive at the piecewise defined function:

- Code: Select all
`if (t <= tb[i])`

se = bmax[i] * (0.25 * t * t + (tb[i] * tb[i] / (8.0 * M_PI * M_PI)) * (cos(2.0*M_PI*t/tb[i])-1.0));

else if (t <= te[i] - tb[i])

se = vmax[i] * (t - 0.5 * tb[i]);

else

se = 0.5 * bmax[i] * (te[i] * (t + tb[i]) - 0.5*(t*t + te[i]*te[i] + 2.0 * tb[i]*tb[i]) + (tb[i]*tb[i]/(4.0 * M_PI * M_PI))*(1.0 - cos((2.0*M_PI/tb[i])*(t - tv))));

tb and tv are the acceleration end-time and deacceleration start-time I referred to earlier.

My no S-Curve comes from 2x integration of the piecewise defined, rectangular function: graph

New Graphs:

no S-Curves

Sine S-Curves

Comparison

***images copied to RoboSavvy.com/Builders/cosa/ again..***

Hi Cosa,

I just got news from my friend that you are developing your robot control, including a 3d visualisation, xml-based robot modelling, a custom scripting language, the capture of robot movement, etc. I am wondering to know how do you manage to handle s-curve problem during the robot motion.

looking forward to hearing from you.

cheers,

ryann2k1

I just got news from my friend that you are developing your robot control, including a 3d visualisation, xml-based robot modelling, a custom scripting language, the capture of robot movement, etc. I am wondering to know how do you manage to handle s-curve problem during the robot motion.

looking forward to hearing from you.

cheers,

ryann2k1

Hi ryann,

the code above works. You just have to make sure that the timing in the uC program is pretty accurate (that's a bit tricky in my program because I do other stuff in parallel, f.e. reading data from the Ax12s and AxS1s and sending it to the pc). If you want to implement simple point-to-point motions the "sine" or "constant" s-curve produces very good results (of course it depends on the number of interpolated points to produce, the pause between two interpolated points, the max velocity and max acceleration).

What exactly do you mean by the s-curve problem? I don't use s-curves at the moment but simple linear interpolation (because it's two times faster) and it works pretty well.

But this depends on the situation: I do most of the interpolation stuff on the pc and the uC only interpolates points sent every 80ms to it. So in the end the trajectory is a linearized version of the smooth trajectory calculated by the pc.

Edit: removed some videos (you can find them here):

Comparison of:

interpolation (of six robot poses) in cartesian space:

I can add the "rectangular" s-curve if someone is interested.

the code above works. You just have to make sure that the timing in the uC program is pretty accurate (that's a bit tricky in my program because I do other stuff in parallel, f.e. reading data from the Ax12s and AxS1s and sending it to the pc). If you want to implement simple point-to-point motions the "sine" or "constant" s-curve produces very good results (of course it depends on the number of interpolated points to produce, the pause between two interpolated points, the max velocity and max acceleration).

What exactly do you mean by the s-curve problem? I don't use s-curves at the moment but simple linear interpolation (because it's two times faster) and it works pretty well.

But this depends on the situation: I do most of the interpolation stuff on the pc and the uC only interpolates points sent every 80ms to it. So in the end the trajectory is a linearized version of the smooth trajectory calculated by the pc.

Edit: removed some videos (you can find them here):

Comparison of:

- linear

sinoidal s-curve (1x acceleration)

sinoidal s-curve (2x)

sinoidal s-curve (4x)

sinoidal s-curve (8x)

sinoidal s-curve (16x)

interpolation (of six robot poses) in cartesian space:

phpBB [media]

I can add the "rectangular" s-curve if someone is interested.

Last edited by cosa on Sat May 17, 2008 10:11 pm, edited 2 times in total.

My first guess is that this is a timing problem. Are you sure that the timing is precise and you send the position data at a constant rate?

Besides a few hacks my interpolation scheme is as simple as this:

- Code: Select all
`ipoData.interpolationPause = 20; // 2ms`

while (isRunning)

{

if (getCurrentTime() >= robotData.readTime + ipoData.interpolationPause)

{

robotData.readTime = getCurrentTime();

doInterpolation();

}

}

Where getCurrentTime() reads the current value of a counter incremented by one of the timers in 100uS intervalls.

If you're doing a lot of stuff besides that you could increment the interpolation pause (according to bullit, robotis uses a pause of 8ms).

Keep me up-to-date!

Hi,

I am developing my own walking gaits. Currently I am developing learning for gaits generation. I have finalized my walking gaits for the bioloid" Angie".,but it is just initial walking gaits. Did you see the sudden change to the speed? I am still wondering how it can happen.

cheers

ryann2k1

I am developing my own walking gaits. Currently I am developing learning for gaits generation. I have finalized my walking gaits for the bioloid" Angie".,but it is just initial walking gaits. Did you see the sudden change to the speed? I am still wondering how it can happen.

cheers

ryann2k1

Sorry for my late reply. A few months ago I've implemented the walking code described in the following paper: http://citeseer.ist.psu.edu/behnke06online.html. It worked quite well (at least walking forward) but since I have changed the robot slightly I have to tune a few parameters. When everything works I'll post a video. If someone is interested in the code (it calculates the joint angles directly and doesn't use inverse kinematics so everyone should be able to use it) I can post it here.

Last edited by cosa on Thu Nov 27, 2008 2:48 pm, edited 1 time in total.