S-Curve Calculation

Bioloid robot kit from Korean company Robotis; CM5 controller block, AX12 servos..
44 postsPage 1 of 31, 2, 3
44 postsPage 1 of 31, 2, 3

S-Curve Calculation

Post by Rob » Thu Apr 24, 2008 10:37 am

Post by Rob
Thu Apr 24, 2008 10:37 am

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
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 offline
Savvy Roboteer
Savvy Roboteer
Posts: 51
Joined: Sun Nov 25, 2007 2:05 pm

Post by cosa » Thu Apr 24, 2008 12:48 pm

Post by cosa
Thu Apr 24, 2008 12:48 pm

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
Image

Sine S-Curves
Image

Comparison
Image


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

Sine S-Curves
Image

Comparison
Image


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.
cosa offline
Savvy Roboteer
Savvy Roboteer
User avatar
Posts: 40
Joined: Mon Dec 04, 2006 1:00 am

Post by Bullit » Thu Apr 24, 2008 6:04 pm

Post by Bullit
Thu Apr 24, 2008 6:04 pm

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 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.
Bullit offline
Savvy Roboteer
Savvy Roboteer
User avatar
Posts: 290
Joined: Wed May 31, 2006 1:00 am
Location: Near robot

Post by cosa » Thu Apr 24, 2008 6:54 pm

Post by cosa
Thu Apr 24, 2008 6:54 pm

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".
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".
cosa offline
Savvy Roboteer
Savvy Roboteer
User avatar
Posts: 40
Joined: Mon Dec 04, 2006 1:00 am

Post by Bullit » Thu Apr 24, 2008 9:46 pm

Post by Bullit
Thu Apr 24, 2008 9:46 pm

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 :D
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 :D
Bullit offline
Savvy Roboteer
Savvy Roboteer
User avatar
Posts: 290
Joined: Wed May 31, 2006 1:00 am
Location: Near robot

Post by cosa » Fri Apr 25, 2008 2:05 pm

Post by cosa
Fri Apr 25, 2008 2:05 pm

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..***
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 offline
Savvy Roboteer
Savvy Roboteer
User avatar
Posts: 40
Joined: Mon Dec 04, 2006 1:00 am

Post by cosa » Fri Apr 25, 2008 2:47 pm

Post by cosa
Fri Apr 25, 2008 2:47 pm

Thanks bullit. The failure was also in my controller program!
Thanks bullit. The failure was also in my controller program!
cosa offline
Savvy Roboteer
Savvy Roboteer
User avatar
Posts: 40
Joined: Mon Dec 04, 2006 1:00 am

Bioloid control

Post by ryann2k1 » Mon Apr 28, 2008 5:59 am

Post by ryann2k1
Mon Apr 28, 2008 5:59 am

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
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
ryann2k1 offline
Savvy Roboteer
Savvy Roboteer
User avatar
Posts: 154
Joined: Thu Nov 16, 2006 1:00 am

Post by cosa » Sun May 04, 2008 6:53 pm

Post by cosa
Sun May 04, 2008 6:53 pm

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.
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.
Last edited by cosa on Sat May 17, 2008 10:11 pm, edited 2 times in total.
cosa offline
Savvy Roboteer
Savvy Roboteer
User avatar
Posts: 40
Joined: Mon Dec 04, 2006 1:00 am

S-curve

Post by ryann2k1 » Tue May 06, 2008 6:36 am

Post by ryann2k1
Tue May 06, 2008 6:36 am

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
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
ryann2k1 offline
Savvy Roboteer
Savvy Roboteer
User avatar
Posts: 154
Joined: Thu Nov 16, 2006 1:00 am

Post by cosa » Tue May 06, 2008 10:10 am

Post by cosa
Tue May 06, 2008 10:10 am

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!
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!
cosa offline
Savvy Roboteer
Savvy Roboteer
User avatar
Posts: 40
Joined: Mon Dec 04, 2006 1:00 am

s-curve

Post by ryann2k1 » Tue May 06, 2008 11:24 am

Post by ryann2k1
Tue May 06, 2008 11:24 am

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
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
ryann2k1 offline
Savvy Roboteer
Savvy Roboteer
User avatar
Posts: 154
Joined: Thu Nov 16, 2006 1:00 am

Post by cosa » Sat May 17, 2008 9:35 pm

Post by cosa
Sat May 17, 2008 9:35 pm

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.
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.
cosa offline
Savvy Roboteer
Savvy Roboteer
User avatar
Posts: 40
Joined: Mon Dec 04, 2006 1:00 am

Post by cosa » Sun May 25, 2008 5:58 pm

Post by cosa
Sun May 25, 2008 5:58 pm

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...
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...
cosa offline
Savvy Roboteer
Savvy Roboteer
User avatar
Posts: 40
Joined: Mon Dec 04, 2006 1:00 am

Post by MoSin » Sun May 25, 2008 6:57 pm

Post by MoSin
Sun May 25, 2008 6:57 pm

cosa wrote: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?
cosa wrote: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?
MoSin offline
Robot Builder
Robot Builder
User avatar
Posts: 11
Joined: Fri Jul 13, 2007 4:37 am
Location: Montreal, Quebec, Canada

Next
Next
44 postsPage 1 of 31, 2, 3
44 postsPage 1 of 31, 2, 3