RoboSavvy Forum

Linear interpolation

2008-11-27T10:29:20+01:00

Ah, of course... that makes sense. I'm obviously a little slow on the uptake.

In an attempt to reduce complexity, I've rearranged the arm to be all in the X/Y plane (as was previously suggested), which allowed me to reduced the number of servos from 5 to 3, and significantly shortened the throw.

Running the original algorithm (calculate speeds for each servo based on target position, SYNC_WRITE speed and position once per pose) seems to work somewhat better with the reduced DOF. It's still more inaccurate than I would like, but at least there are less variables to pay attention to.

I'll try implementing your suggestion. Thanks!

Statistics: Posted by petej — Thu Nov 27, 2008 10:29 am

Linear interpolation

2008-11-27T05:03:13+01:00

How would this work? You have to have different speeds on each servo in order to have all the servos reach the destination at the same time, and in order to ever implement an S-curve, you'd have to adjust the speed continually during the ramps, right?

You can control the average speed of the servo by sending position values to it. If you assume/set a pretty high acceleration/speed the servo will be able to reach every position within a few milliseconds. Then the average speed just depends on the position values you send in a specific time frame (f.e. the values 100 101 102 will be slower than 100 200 300). Of course, the problem is, that the ax-12 controller tries to reach every position with a fixed acceleration and max velocity which results in a jerky motion. But the higher your update frequency is the smaller the error of the resulting trajectory will be. Therefore you can limit the jerk by using small timesteps and a smooth precalculated trajectory (which should take the dynamics into account).

For me, a timestep of ~10ms, a smooth trajectory (f.e. cubic spline or s-curve) and (optional) a simple pid controller for the torque value works okay, but I don't want to draw straight lines. I don't have up-to-date videos but you can find a few old ones here.

You'll get better results if you take the dynamics into account and calculate "correct" position, speed and torque values but it's not that easy (and you'd need information about how the ax-12 controller works). Using max torque and max velocity has the advantage that you can't underestimate the values

Statistics: Posted by cosa — Thu Nov 27, 2008 5:03 am

Linear interpolation

2008-11-22T23:28:26+01:00

Most of the controller code I have seen does not use the speed control ability of the servo. The servo runs at max speed.

An interrupt timer is set to a few millisec, and on the interrupt the servo position only is set.

I am thinking that setting the sped often, may mess with the servo internal code.

Might I suggest a timer based position only program to start and then to experiment with adding the speed.

You know, this matches what Bullit said in this thread:

... the bioloid firmware sets the compliance slope to 0x20,0x20 whenever it plays a page. Margin is left at 0. Speed is set to 0, torque is set to 0x3ff.

How would this work? You have to have different speeds on each servo in order to have all the servos reach the destination at the same time, and in order to ever implement an S-curve, you'd have to adjust the speed continually during the ramps, right?

Further, in the BLV012 code from the University of Plymouth, they send the goal speed with every packet, calculated in a similar way as my code (I derived mine from theirs).

Statistics: Posted by petej — Sat Nov 22, 2008 11:28 pm

Linear interpolation

2008-11-22T19:07:17+01:00

i-Bot wrote:
I am thinking that setting the sped often, may mess with the servo internal code.

I'm not sure if you were referring to my post above, but either way I should explain a bit more..

By velocity I mean tracking velocity and position yourself. The way you set the servo can be by setting positions, just that you are setting the position not exact, but more based upon the direction where you want to go.

Statistics: Posted by billyzelsnack — Sat Nov 22, 2008 7:07 pm

Linear interpolation

2008-11-22T10:54:17+01:00

Most of the controller code I have seen does not use the speed control ability of the servo. The servo runs at max speed.

An interrupt timer is set to a few millisec, and on the interrupt the servo position only is set.

I am thinking that setting the sped often, may mess with the servo internal code.

Might I suggest a timer based position only program to start and then to experiment with adding the speed.

Statistics: Posted by i-Bot — Sat Nov 22, 2008 10:54 am

Linear interpolation

2008-11-22T00:23:40+01:00

I've not actually got to the point where I've solved the problem. I have some ideas though ( of course! haha. )

If you switched over to controlling velocity instead of position ( treat the servo like a motor with position feedback ) you'd at least have something to tweak. Unfortunately this is like implementing a PID on top of the servo's PID which is less than ideal.

Statistics: Posted by billyzelsnack — Sat Nov 22, 2008 12:23 am

Linear interpolation

2008-11-23T19:40:32+01:00

When you send a position you are not only telling the servo to go to that position, but to also stop at that position. If you send lots of position updates then the servo is constantly going and stopping.

Yeah, that's how I'd explain what's happening now. My timing is wrong.

My question is about what the factors are that I should consider when trying to determine the exact timing for sending the next position? How have other people solved this problem?

For example, the manual says at 10V, the max speed of the servo is 60 degrees in 0.196 seconds. I haven't actually tested the voltage on the line (duh), but I'm guessing I'm running at about the battery voltage (9.6V?). So does a speed setting of 1023 constitute a speed of 60 degrees in 0.196 seconds? If the servo is not matching that spec'd output, then I guess I can't calculate anything reliably.

Another thing to consider is how long the communication takes. I guess I have to do the math and see how long it take to transmit that SYNC_WRITE packet at 1Mbps. Assuming the servos start moving immediately after the last byte of the packet is received, I need to start sending the packet that much earlier than the frame start time.

Am I over-complicating this? I guess my case is sort of special, since most people seem to be acting on Motion512 data, which seems to have well-understood characteristics (from Bullit and others). I sort of figured my poses must be effectively the same as the data created from the Motion Editor, as long as factors like pose speed, etc were not moved from defaults.

I'm starting to feel like my Bioloid arm is judging me. He just sits there, day after day, looking at me with that... look.

Edit: I tested Vdd when connected to the wall, and it reads about 12.1V, so that might add some amount of error in the calculation.

Statistics: Posted by petej — Fri Nov 21, 2008 11:09 pm

Linear interpolation

2008-11-21T20:21:57+01:00

When you send a position you are not only telling the servo to go to that position, but to also stop at that position. If you send lots of position updates then the servo is constantly going and stopping.

Statistics: Posted by billyzelsnack — Fri Nov 21, 2008 8:21 pm

Linear interpolation

2008-11-21T18:09:51+01:00

Have you already tried to interpolate the joint angles and create intermediate frames? You won't have to change much if you do it in joint space (add an outer loop t=0..1, store end-start and do target=start + t*(end-start)).

I've started trying to do this, but I seem to be calculating the necessary time between moves incorrectly. Instead of moving smoothly, it jerks between frames. I know I'm not accounting for any communication times, but is there anything else I should be taking into account here?

Code:: static void move_to_pose(uint16_t *pose) { int i, j; int rc; uint16_t current[NUM_SERVOS]; int16_t deltas[NUM_SERVOS]; uint16_t speeds[NUM_SERVOS]; uint8_t fields[NUM_SERVOS * 4]; uint16_t max_delta = 0; float w; float x; float t; // find the current positions for (i=0; i { rc = dynamixel_get_position(g_state.servo_ids[i], ¤t[i]); deltas[i] = pose[i] - current[i]; if (abs(deltas[i]) > max_delta) max_delta = abs(deltas[i]); } // our MAX_SPEED is 0x10 units/sec // from ax-12 manual // wmax = 60 deg / 0.196 sec @ 10V // wmax = 306.1 deg/sec // max range is 300 deg or 1024 units // 1 deg x units // ------- = ---------- // 300 deg 1024 units // x = 1024/300 // x = 3.41 units/deg // wmax = 3.41 units/deg * 306.1 deg/sec // wmax = 1043.8 units/sec // t = max_delta / w; w = 306.1*MAX_SPEED / 1024.0; x = 1024/300.0; t = max_delta / (x * w); // calculate transit speeds for (i=0; i { speeds[i] = (abs(deltas[i]) / (float) max_delta) * MAX_SPEED; // speed of 0 means largest possible if (speeds[i] == 0) speeds[i] = 1; } // cut it into frames for (i=0; i { for (j=0; j { int position_idx = j * 4; int16_t segment = deltas[j] / (float) NUM_FRAMES; uint16_t position = current[j] + (segment * i) + segment; fields[position_idx + 0] = position & 0xff; fields[position_idx + 1] = (position >> 8) & 0xff; fields[position_idx + 2] = speeds[j] & 0xff; fields[position_idx + 3] = (speeds[j] >> 8) & 0xff; } rc = dynamixel_tx_sync_write(DYNAMIXEL_TABLE_GOAL_POSITION_L, NUM_SERVOS, g_state.servo_ids, 4, fields); _delay_ms(t * 1000 / NUM_FRAMES); } }

Statistics: Posted by petej — Fri Nov 21, 2008 6:09 pm

Linear interpolation

2008-11-19T19:46:59+01:00

You're right, sorry

Edit: Have you already tried to interpolate the joint angles and create intermediate frames? You won't have to change much if you do it in joint space (add an outer loop t=0..1, store end-start and do target=start + t*(end-start)).

Statistics: Posted by cosa — Wed Nov 19, 2008 7:46 pm

Linear interpolation

2008-11-19T18:00:37+01:00

You have to use the maximum of all differences because you want all your servos to finish at the same time (which produces nicer arcs as you noticed).

Really? Using the max possible difference should produce the same result as the max of all differences, as far as arriving at the destination at the same time. They would produce different results when it comes to how fast they move there, but they should all still arrive at the same time. Or do I misunderstand?

Statistics: Posted by petej — Wed Nov 19, 2008 6:00 pm

Linear interpolation

2008-11-19T13:20:44+01:00

Looks good to me
Perhaps you'll get slightly better results if you tilt your arm by 90° so that all servos point into the same direction (then they don't have to compensate the gravitational forces). But the main problem will be the resolution of the ax-12.

You have to use the maximum of all differences because you want all your servos to finish at the same time (which produces nicer arcs as you noticed).

Statistics: Posted by cosa — Wed Nov 19, 2008 1:20 pm

Linear interpolation

2008-11-19T10:59:52+01:00

So I tried slowing it down, where the max speed it could achieve was 0x20 instead of 0x03ff. At that speed I could also safely use the max of the current deltas to normalize, rather than a constant. It seems to draw much better lines.

I also tried a super-hacky method of sending multiple "correction" updates along the transit. Basically, I just slept for a bit after sending the SYNC_WRITE, and then re-ran the whole query/write process if the last max delta was greater than some threshold. It really didn't seem to improve anything.

Here are the new lines:

The closer lines are where I used the multiple SYNC_WRITEs per transit. The further one is the same set of parameters, but just one SYNC_WRITE. They look effectively the same to me... in fact, I'd almost claim that the latter looks better.

In another test, I just slowed it down further to 0x10, and it's almost perfect. I guess there's just a lot of inertia/momentum/forces at play, especially given how much leverage an arm that long has.

Running short transits, you can't see the same error at higher speeds, which maybe explains why the humanoid motions don't suffer from this?

I guess all of this further supports that failed dynamics class back in school.

Statistics: Posted by petej — Wed Nov 19, 2008 10:59 am

Linear interpolation

2008-11-19T07:23:06+01:00

Wow, thanks for your responses!

The set of values used for this drawing are:

servo IDs:
{ 3, 7, 11, 13, 15, }

servo positions:
{ 0x0131, 0x02d3, 0x012d, 0x02d9, 0x0201 },
{ 0x00be, 0x0342, 0x00bc, 0x034c, 0x0202 },
{ 0x00ef, 0x0342, 0x00f0, 0x0343, 0x0298 },
{ 0x0176, 0x0290, 0x0179, 0x028f, 0x026c },

The order of the IDs above is in ascending order. The real order of the servos, from CM-5 out, is 15, 11, 3, 13, 7. I didn't put them in order in this post (which would make entering them easier I'm sure) because I didn't want to risk adding copy/paste errors.

MAX_DELTA is constant at 0x3ff, not the max of the current deltas. The problem I figured with using the max of the current deltas is that when I scaled up the normalized values for speed, a small max delta could end up driving at full speed causing massive overshoot. I'll give this a whirl anyway.

bullit: Thanks for the link. Those plotters were actually my "inspiration", but I wanted to stick with C.

Statistics: Posted by petej — Wed Nov 19, 2008 7:23 am

Linear interpolation

2008-11-19T04:38:55+01:00

oh I see. Your start/end are in angles. What I meant was that the end point position being closer to the base. So it is meant to draw a sloped line wrt the base.

Statistics: Posted by billyzelsnack — Wed Nov 19, 2008 4:38 am