Move from Point A to B with given steps


I’m trying to implement a function in C# which move each joint from an initial position A to a target position B, using a step by step movement. Steps are defined as how much the joint should be altered to reach the point B.

Consider joint zero going from A (-150º) to B (30º). The difference between A to B is 120º, with a 20º increment, the joint zero should be incremented in 6 steps. But it goes from minus to plus and I can’t just sum up 20º to the original position A.

Any available code or information how to achieve this would be very appreciated. I’m very thankful in advance,


André Castro.

The distance between A and B is 180º.

B - A = 30 - (-150) = 180

So some C# to calculate the interpolated values based on a resolution is thus:

public static List<double> InterpolateBetween(double start, double end, int steps)
    List<double> positions = new List<double>();
    double delta = end - start;
    double increment = delta / (double)steps;
    for (int i = 1; i <= steps; i++)
        double position = start + ((double)i * increment);

    return positions;


Which, passing in arguments of -150, 30 and 9, givens the list:










Hey, @90jb12,

you’re right, my mistake :expressionless:

Thank you for the provided snippet, this should solve the issue. Yet I guess we should take the absolute value from delta, in case we have a start position positive and the end negative.


A= 150 and B=-30

B – A = -30 -150 = -180

double delta = Math.Abs(end - start);


André Castro.


Not entirely!

If that was the case, it would increment in the wrong direction. Allowing delta to be a negative, makes sure the increment calculation goes in the correct direction:

double position = start + ((double)i * increment);

If delta; and subsequently increment, are only allowed to be positive, this would happen:

170, 190, 210… etc

When increment is negative however, it goes: 130, 90, 70, 50… etc


Hope this helps!

Now I see it crearly. Thanks a lot, mate!