Now that I have the inverse kinematics worked out for my robot arm, I need to work on having it move from point A to point B in a straight line.
I am wondering what is the best way to do this?
Should I calculate fractions of movement along a line in space between point A and point B? That seems like the best way to do it, analogous to the way the route is calculated in drawing an arc. You determine the resolution or line length, and divide up the task into increments based on the number of such increments into the total distance, then step the arm to points calculated by IK along that line.
My goal is to have the arm travel in a straight line at least along the X and Y axis, while maintaining Z height.
That is how I ran each of the legs of my hexapod.
I planned out the entire foot path (which resembled the shape of a tank tread) and broke it into an array of 200 points. Then I called my IK routine on each XYZ point and sent the updates to the servos (on a timed bases to match the servo signal frequency.
You can even do acceleration in that if you take smaller steps at each end of the movement with larger steps in the middle.
Edit - You might want to time the calculation of the IK process. Grab Millis, run 100 IK calculations and then grab millis again. Print out the start, end and duration. That way if you run into performance issues later, you will have some idea of what is the bottle neck. As I recall, Arctan is an expensive routine. We used approximation versions of Arctan and got much faster times.
Speed is definitely an issue. For drawing an arc, it isn't at all unusual to have a 1000 points, all of which must go through a three step IK calculation for the three axis arm. Good thing I'm not trying to make it catch a ball or anything. I will continue to experiment with the resolution, which is currently set at .1 mm movements per point. That's probably too fine of resolution and a waste of processing time, but its something I can easily play around with later. I found that debugging serial.print comments really bogged things down.
Here's some code I found written in C#
private Point[] PointsAlongLine(Point start, Point end, int spacing) {
int xDifference = end.X - start.X;
int yDifference = end.Y - start.Y;
int absoluteXdifference = Math.Abs(start.X - end.X);
int absoluteYdifference = Math.Abs(start.Y - end.Y);
int lineLength = (int)Math.Sqrt((Math.Pow(absoluteXdifference, 2) + Math.Pow(absoluteYdifference, 2))); //pythagoras
int steps = lineLength / spacing;
int xStep = xDifference / steps;
int yStep = yDifference / steps;
Point[] result = new Point[steps];
for (int i = 0; i < steps; i++) {
int x = start.X + (xStep * i);
int y = start.Y + (yStep * i);
result[i] = new Point(x, y);
}
return result;
}
Most people use the Bresenham algorithm to generate points along a line. It is accurate, and can use integer only calculations for speed.
Variations of the algorithm can be used to generate points along just about any curve.
I'm wondering what variation of Bresenhams algorithm would work to generate smaller increments than integers. Simply moving from x=4 to x=5 is way too rough of a line. I need to be able to set the increments to at least 0.1 units. Do I use Bresenhams and use a conversion to 10x then divide again to use the results?
Does your IK logic take inter input? I used floats. Divide it up into however many slices you want.
Here, your example divided into 10 slices.
4.0
4.1
4.2
...
5.0
I've been experimenting with this code. I will need to solve the issue of incrementing it by 0.1 instead of by 1.
Perhaps I can multiply all coordinates by 10 prior to input, and then divide by 10 on the output to give me 0.1 cm points along the line.
[code]
// Bresenham line algorithm prints out from x1,y1 to x2,y2 in whole numbers
void setup() {
// put your setup code here, to run once:
Serial.begin(57600);
}
void loop() {
// put your main code here, to run repeatedly:
Bresenham (3, 6, 5, -6);
}
void Bresenham(int x1, int y1,int const x2,int const y2)
{
int delta_x(x2 - x1);
// if x1 == x2, then it does not matter what we set here
signed char const ix((delta_x > 0) - (delta_x < 0));
delta_x = abs(delta_x) << 1;
int delta_y(y2 - y1);
// if y1 == y2, then it does not matter what we set here
signed char const iy((delta_y > 0) - (delta_y < 0));
delta_y = abs(delta_y) << 1;
// plot(x1, y1);
Serial.print ("x1= ");
Serial.println (x1);
Serial.print ("y1 = ");
Serial.println (y1);
if (delta_x >= delta_y)
{
// error may go below zero
int error(delta_y - (delta_x >> 1));
while (x1 != x2)
{
// reduce error, while taking into account the corner case of error == 0
if ((error > 0) || (!error && (ix > 0)))
{
error -= delta_x;
y1 += iy;
}
// else do nothing
error += delta_y;
x1 += ix;
// plot(x1, y1);
Serial.print ("x1= ");
Serial.println (x1);
Serial.print ("y1 = ");
Serial.println (y1);
}
}
else
{
// error may go below zero
int error(delta_x - (delta_y >> 1));
while (y1 != y2)
{
// reduce error, while taking into account the corner case of error == 0
if ((error > 0) || (!error && (iy > 0)))
{
error -= delta_y;
x1 += ix;
}
// else do nothing
error += delta_x;
y1 += iy;
// plot(x1, y1);
Serial.print ("x1= ");
Serial.println (x1);
Serial.print ("y1 = ");
Serial.println (y1);
}
}
}
[/code]
Do I use Bresenhams and use a conversion to 10x then divide again to use the results?
That will work fine and is much faster than using floats.
What is this supposed to do?
int error(delta_x - (delta_y >> 1));
I honestly don't know. :o I can't take credit or blame. Some people write code that goes way over my head. I've noticed some code that uses (math for instance) functions so obscure there is scarcely a mention of it to be found on the internet. At least this code appears to work, as I've tested it with a few different slopes and directions of travel.
Can you direct me to an example of a Bresenham algorithm written in C++ that works for lines going in any direction and slope?
This seems to work with every combination of numbers I throw at it, and I've tested it with the divided by 10 concept and it works. Should allow my to plot points every mm of a straight line.
[code]
// Bresenham line algorithm prints out from x1,y1 to x2,y2 in whole numbers
boolean executed = false; // boolean variable to check if move is completed
int x1;
int y1;
float newx1;
float newy1;
void setup() {
// put your setup code here, to run once:
Serial.begin(57600);
}
void loop() {
// put your main code here, to run repeatedly:
if (executed == false ) {
Bresenham (65, -60, -50, 60);
executed = true;
}
}
void Bresenham(int x1, int y1, int const x2, int const y2)
{
int delta_x = (x2 - x1);
// if x1 == x2, then it does not matter what we set here
signed char const ix((delta_x > 0) - (delta_x < 0));
delta_x = abs(delta_x) << 1;
int delta_y = (y2 - y1);
// if y1 == y2, then it does not matter what we set here
signed char const iy((delta_y > 0) - (delta_y < 0));
delta_y = abs(delta_y) << 1;
plot(x1, y1);
if (delta_x >= delta_y)
{
// error may go below zero
int error = (delta_y - (delta_x >> 1));
while (x1 != x2)
{
// reduce error, while taking into account the corner case of error == 0
if ((error > 0) || (!error && (ix > 0)))
{
error -= delta_x;
y1 += iy;
}
// else do nothing
error += delta_y;
x1 += ix;
plot(x1, y1);
}
}
else
{
// error may go below zero
int error = (delta_x - (delta_y >> 1));
while (y1 != y2)
{
// reduce error, while taking into account the corner case of error == 0
if ((error > 0) || (!error && (iy > 0)))
{
error -= delta_y;
x1 += ix;
}
// else do nothing
error += delta_x;
y1 += iy;
plot(x1, y1);
}
}
}
void plot(int x1, int y1) {
float newx1 = (float)x1 / 10.0;
float newy1 = (float)y1 / 10.0;
Serial.print ("x1= ");
Serial.print (newx1);
Serial.print (" y1 = ");
Serial.println (newy1);
}
[/code]
Glad you got it working!
I had to look up that unusual line (one of a couple of such in the code) and it is a "constructor initialization statement" as follows.
A second method, known as constructor initialization (introduced by the C++ language), encloses the initial value between parentheses (()):
type identifier (initial_value);
For example:
int x (0);
I see no reason to use that notation, along with the completely unhelpful comment, as the following also works and has clear meaning:
int ix = (delta_x > 0) - (delta_x < 0); //x increment = 1, 0 or -1
Modified and tested code:
// Bresenham line algorithm prints out from x1,y1 to x2,y2 in whole numbers
int x1;
int y1;
float newx1;
float newy1;
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
Bresenham (65, -60, -50, 60);
}
void loop() {}
void Bresenham(int x1, int y1, int const x2, int const y2)
{
int delta_x = (x2 - x1);
int ix = (delta_x > 0) - (delta_x < 0); //x increment = 1, 0 or -1
// Serial.print(" delta_x = ");
// Serial.println(delta_x);
// Serial.print(" ix = ");
// Serial.println(ix);
delta_x = abs(delta_x) << 1;
int delta_y = (y2 - y1);
int iy = (delta_y > 0) - (delta_y < 0); //y increment = 1, 0 or -1
// Serial.print(" delta_y = ");
// Serial.println(delta_y);
// Serial.print(" iy = ");
// Serial.println(iy);
delta_y = abs(delta_y) << 1;
plot(x1, y1);
if (delta_x >= delta_y)
{
// error may go below zero
int error = (delta_y - (delta_x >> 1));
while (x1 != x2)
{
// reduce error, while taking into account the corner case of error == 0
if ((error > 0) || (!error && (ix > 0)))
{
error -= delta_x;
y1 += iy;
}
// else do nothing
error += delta_y;
x1 += ix;
plot(x1, y1);
}
}
else
{
// error may go below zero
int error = (delta_x - (delta_y >> 1));
while (y1 != y2)
{
// reduce error, while taking into account the corner case of error == 0
if ((error > 0) || (!error && (iy > 0)))
{
error -= delta_y;
x1 += ix;
}
// else do nothing
error += delta_x;
y1 += iy;
plot(x1, y1);
}
}
}
void plot(int x1, int y1) {
float newx1 = (float)x1 / 10.0;
float newy1 = (float)y1 / 10.0;
Serial.print ("x1= ");
Serial.print (newx1);
Serial.print (" y1 = ");
Serial.println (newy1);
}
The challenge I am facing right now is how to implement multistepper and use the run command. Run is non-blocking, so it permits me to stop the robotic arm if it reaches any limit switches, but it appears run requires placement in a loop, as its not a call it once and leave it command, like runSpeedToPosition, which is blocking and only needs to be called once. I like multistepper because it coordinates all the movements to end at the same time.
So in the above code, if I replace plot() with a function that sets up movement with multistepper's run, it seems to only move while it processes the math for the Bresenham, which stops the movement short.
Got it working. I am using the blocking version of multistepper's run, being runSpeedToPosition. I have found that I have to break the movements down to 1/100th of a cm to get smooth stepping. I tried 1/10th and 1/50th and it makes to much vibration. It's amazing that despite having to do all the inverse kinematics 100 times for every cm of movement, it still moves rapidly. My ultimate goal is to be able to scan an image, convert it to Gcode and have the arm make the drawing. All just for the fun of it.