S-Curve Calculation

[Rob]

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

[cosa]

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

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);
}

[Bullit]

cosa it looks like your s-curve calculator will be kind of slow since it only touches peak velocity in the center. Typically s-curves have a constant velocity phase, depending on accel, decel times. The whole idea of an s-curve is to minimize jerk. Its a good model in absence of an true inertial model. Constant acceleration is also called a trapazoidal model and although common in many motor control systems is not nearlly as smooth as an s-curve. Where the s-curve breaks down is where inertia is a significant factor. In this case intertia can drive over shoot in either the trapazoidal or s-curve model.

[cosa]

There is a constant velocity phase (edit: should be ). Acceleration end time and deacceleration start time is at 25% and 75% of the total time (it's hard to see though). When I'm at home, I'll make new pictures.
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".

[Bullit]

cosa, a typical s-curve should be increasing acceleration, decreasing acceleration, constant velocity, increasing deceleration, decreasing deceleration. The function is parabolic not sinusoidal.
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

[cosa]

You are right: there is no constant velocity phase in the pictures because there was a flaw in the calculation of tb (an error occured and tb was always set to exactly the half of the total time).
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..***

[cosa]

Thanks bullit. The failure was also in my controller program!

[ryann2k1]

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

[cosa]

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:

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.

[ryann2k1]

Hi there,
Nice clips.
Have you tried on walking?
I have to struggle with the s-curve issues. And I have really strange moves,there is a certain time when the walk is accelerated accidentally and it is really disturbing the walking patterns.

phpBB [media]

ryann2k1

[cosa]

Hey, your robot looks cool. Is this the original walking pattern by robotis and you want to implement your own s-curve mechanism to play it or do you use your own motion system?

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!

[ryann2k1]

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

[cosa]

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.

[cosa]

Here's the video:
Bioloid Omnidirectional Walking:
1. 6 steps forward (1x speed)
2. 8 steps forward and left (1x speed)
3. 20 steps forward and left (0.5x speed, same curve radius)
4. 6 steps forward (1x speed)

phpBB [media]

It's still a bit unsteady...

[MoSin]

It's still a bit unsteady...

well, it is pretty impressive even if a little bit unsteady. Did you make it go backwawrd with the same technique used?