Linear interpolation

[petej]

Hey there...

I've got this arm that I built:



The idea is currently that you can move the arm manually to a location, then save the positions of all the servos as a pose (like motion editor, but in the CM-5 RAM). Once you have a set of poses saved, you can play them back (currently one at a time).

So that's great, and I've got it effectively working, but it doesn't move properly between points. Here's an example:



This drawing has four points at the corners, which I would like to have straight (arc'd) lines between but the line on the left is all squiggly, and I'm having some difficulty understanding what I need to do to make it straight.

I don't think I'm doing anything fancy in the the math I'm using to drive the motors. I use a SYNC_WRITE to set the speed of each servo relative to the normalized position delta. Here's the relevant code:

Code: Select all

    for (i=0; i<NUM_SERVOS; i++)
    {   
        int position_idx = i * 4;

        rc = dynamixel_get_position(g_state.servo_ids[i], &current_l, &current_h);

        position_l = pose[i] & (uint8_t) 0xff;
        position_h = (pose[i] >> 8) & (uint8_t) 0xff;

        fields[position_idx + 0] = position_l;
        fields[position_idx + 1] = position_h;

        debug_tx_str("position: 0x");
        debug_tx_hex((uint8_t) (pose[i] >> 8));
        debug_tx_hex((uint8_t) pose[i]);
        debug_tx_crlf();

        deltas[i] = abs((position_l | (position_h << 8)) - (current_l | (current_h << 8)));

        debug_tx_str("delta: 0x");
        debug_tx_hex((uint8_t) (deltas[i] >> 8));
        debug_tx_hex((uint8_t) deltas[i]);
        debug_tx_crlf();
    }   

    for (i=0; i<NUM_SERVOS; i++)
    {   
        uint16_t speed_idx = i * 4 + 2;

        uint16_t speed = (deltas[i] / (float) MAX_DELTA) * MAX_SPEED;

        debug_tx_str("speed: 0x");
        debug_tx_hex((uint8_t) (speed >> 8));
        debug_tx_hex((uint8_t) speed);
        debug_tx_crlf();

        // speed of 0 means largest possible
        if (speed == 0)
            speed = 1;

        fields[speed_idx + 0] = (uint8_t) speed;
        fields[speed_idx + 1] = (uint8_t) (speed >> 8);
    }   

    rc = dynamixel_tx_sync_write(DYNAMIXEL_TABLE_GOAL_POSITION_L, NUM_SERVOS, g_state.servo_ids, 4, fields);


Does anyone see something obvious here (besides the crappy C code)?

Could this be a result of the sort of set-and-forget nature of how I'm doing these moves Like maybe during these long transits there is some major error accumulation, and if I were to continually sent down updated speeds, it would result in straighter lines. Now that I think about it, maybe I'm answering my own question...

Anyway, thanks in advance for any suggestions. I'm going to try the continuous updates tonight.

[billyzelsnack]

My guess is that your end points to the line on the right require less change in the reach servos than the line on the left. Because of that your servos might actually need to be working along a more cos function than a linear function to draw the line. You are dealing with rotations to make translations after all. Everything might be working perfectly.

[cosa]

Hmm, you should get an arc. I've modelled your motion with my program:
Startpose: first to last servo in degree: 0 -10 10 10 -10

Endpose: 45 45 -45 -45 45

Trajectory:

The arm moves on an arc (Edit: updated picture).

If your MAX_DELTA is the maximum of all distances abs(position[i] - current[i]) and not constant then your code looks good. In this case your problem might be the result of the ax12 controller (Edit: is the speed > 0 for every servo?). Therefore I wouldn't rely on the speed value to get a nice arc. If you want to control the trajectory between the start pose and the target pose you have to create intermediate frames. For example, the robotis firmware updates the target positions of the servos every 8ms (according to bullit).
If you want a straight line connection between points in cartesian space you'd have to get the x-y coordinates of the start point s and end point e on the sheet by calculating forward kinematics (if you measure the length of every arm piece you can calculate the coordinates based on the measured joint values). You can model your robots kinematic structure by using the denavit hartenberg rules. Then you could calculate the straight line r(t) = s + t * (e - s) between the two points and create intermediate points by using r(e/n) with e = 0 .. n. Finally you'd have to calculate the joint angles which will lead to the position r(e/n) and send them one by one to the robot (inverse kinematics, f.e. angles[0] = atan2(x,y), angles[1] = ...).

Edit: If you give me the start and end joint angles and the length of your arm parts I can calculate the trajectory (to see how it should look like).

[Bullit]

You also might want to check this out
http://robosavvy.com/forum/viewtopic.php?t=318

Some really nice work similar to yours done by VanHelsing

[billyzelsnack]

cosa.. Make it so one of the endpoints is closer to the base and see what happens.

[cosa]

The startpose is already closer to the base than the endpose or what do you mean?

I think as long as the last four servos are "symmetrical" (like in my example, the links are parallel to the table) there won't be a problem. If they aren't anything can happen F.e., the linear interpolation could lead to an intermediate pose which is below the table. Perhaps petej posts the real values and I can test it.

[billyzelsnack]

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.

[petej]

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.

[petej]

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.

[cosa]

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).

[petej]

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?

[cosa]

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)).

[petej]

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: Select all

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<NUM_SERVOS; i++)
    {   
        rc = dynamixel_get_position(g_state.servo_ids[i], &current[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<NUM_SERVOS; 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<NUM_FRAMES; i++)
    {
        for (j=0; j<NUM_SERVOS; 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);
    }
}


[billyzelsnack]

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.

[petej]

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.