Posted: Thu Apr 24, 2008 9:37 am Post subject: S-Curve Calculation
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?
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:
// 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 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;
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;
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];
Joined: 31 May 2006 Posts: 289 Location: Near robot
Posted: Thu Apr 24, 2008 5:04 pm Post subject:
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.
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".
Joined: 31 May 2006 Posts: 289 Location: Near robot
Posted: Thu Apr 24, 2008 8:46 pm Post subject:
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.
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:
Posted: Mon Apr 28, 2008 4:59 am Post subject: Bioloid control
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.
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):
Posted: Tue May 06, 2008 5:36 am Post subject: S-curve
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.
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:
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).
Posted: Tue May 06, 2008 10:24 am Post subject: s-curve
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.
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 1:48 pm; edited 1 time in total
You cannot post new topics in this forum You cannot reply to topics in this forum You cannot edit your posts in this forum You cannot delete your posts in this forum You cannot vote in polls in this forum
£600.00 inc. VAT
). 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.
