Page 1 of 1

Reputation:

# Dijkstra's Algorithm Implementation?

Posted 19 April 2010 - 05:05 PM

Well I have been working on an assignment that requires me to move the head of a robotic arm from 1 place to another using only the angles at which those 2 joints are currently at, and the arm can only move in 2 dimensions. I completed that part of the assignment and have it working properly, but now we have to modify the code to reach to a certain x,y coordinate to grab something, but there is an obstacle in the way. I have made an 80x80 array that represents the space and whether or not the arm is able to reach that area or not.
I was told that Dijkstras algorithm could help, finding the shortest path to the object that we want to pick up, but I don't really know how to implement that into my problem!

I attached my code so far and what the second assignment is exactly... any suggestions are appreciated, I'm stumped!

```/*
Lance K
Robotics Lab One
2/1/2010
*/

#include <stdio.h>
#include <math.h>

#define POST_LENGTH 100
#define ARM_LENGTH 80
#define PI 3.141592654

struct configInfo
{
//Struct containing the necessary information that will be stored in the 81x81 array.
int allowed;
double a;
double b;
double x;
double y;

};

double getHandX(double alpha, double beta)
{
//Calculates the x coordinate of the hand given 2 angles, alpha and beta.
double delta = PI - (beta + alpha);
return(ARM_LENGTH * sin(-alpha) + ARM_LENGTH * sin(-delta));
}

double getHandY(double alpha, double beta)
{
//Calculates the x coordinate of the hand given 2 angles, alpha and beta.
double delta = PI - (beta + alpha);
return(ARM_LENGTH * cos(-alpha) + POST_LENGTH - ARM_LENGTH * cos(-delta));
}

void getLoc()
{
//Prints the location of the robot gripper in x-y space given 2 angles by the user.
double inputAlpha;
double inputBeta;
printf("Enter the angle of the first joint");
scanf("%lg", &inputAlpha);
printf("Enter the angle of the second joint");
scanf("%lg", &inputBeta);
printf("the location of the hand is (%f , %f) \n,", getHandX(inputAlpha,inputBeta), getHandY(inputAlpha,inputBeta));

}

double getJoint2XLocation(double alpha)
{
return(-80*sin(alpha));
}

double getJoint2YLocation(double alpha)
{
return(80*cos(alpha) + 100);
}

void fillMap(struct configInfo map[81][81])
{

//Breaks the configuration space from -pi to +pi into 81 configInfo objects in each direction(x and y).
int i,j;
for(i = 0; i < 81; i++)
{
for(j = 0; j < 81; j++)
{
//Assigns an alpha and beta angle into each of the configInfo nodes depending on where in the
//array the node lies.
map[i][j].b = -PI + j*(PI/40);
map[i][j].a = -PI + i*(PI/40);

//Determines the (x,y) coordinates of the hand depending on where the hand is in configuration space and
//assigns the values into the corresponding node in the configInfo array.
map[i][j].x = getHandX(map[i][j].a,map[i][j].B)/>;
map[i][j].y = getHandY(map[i][j].a,map[i][j].B)/>;

//Sets the value of allowed for each node to false if these conditions apply...
//Under the ground.
if(map[i][j].y < 0)
{
map[i][j].allowed = 0;
}
//Through base of arm.
else if(map[i][j].a < 0 && map[i][j].b < 0 && map[i][j].x < 0)
{
map[i][j].allowed = 0;
}
//Through base of arm.
else if(map[i][j].a > 0 && map[i][j].b > 0 && map[i][j].x > 0)
{
map[i][j].allowed = 0;
}
//Arm is only allowed a flexibility of 7/8 pi.
else if(map[i][j].a > (7*PI/8) || map[i][j].a < (-7*PI/8))
{
map[i][j].allowed = 0;
}
//Arm is only allowed a flexibility of 7/8 pi.
else if(map[i][j].b > (7*PI/8) || map[i][j].b < (-7*PI/8))
{
map[i][j].allowed = 0;
}
else if(crossLine(map[i][j].a, map[i][j].B)/> == 1)
{
map[i][j].allowed = 0;
}
else
{
map[i][j].allowed = 1;
}

}
}
}

int crossLine(double alpha, double beta)
{
//x1, y1 and x2,y2 are the endpoints of a line extending from joint 2 to the robot's hand.

int isAllowed = 1;
double x1, y1, x2, y2;

x1 = getJoint2XLocation(alpha);
y1 = getJoint2YLocation(alpha);

x2 = getHandX(alpha, beta);
y2 = getHandY(alpha, beta);

double z = (100 - x1)/(x2 - x1);
double temp = z*(y2 - y1) + y1;

/*
printf("Line 1 endpoint 1: %f, %f", x1, y1);
printf("\nLine 1 endpont 2: %f, %f", x2, y2);
printf("\nLine 2 endpoint1: %f, %f", u1, v1);
printf("\nLine 2 endpoint2: %f, %f", u2, v2);
printf("");
*/

if (x1 < 100 && x2 < 100)
{
isAllowed = 0;
}
else if(y1 < 150 && y2 < 150)
{
isAllowed = 1;
}
else if(temp > 150)
{
isAllowed = 0;
}
else
isAllowed = 0;

return isAllowed;

}

void printMap(struct configInfo map[81][81])
{
//Prints the entire 81x81 array showing a "." if the coordinates are accessable and "#" if they are not with the top
//left of the printout corresponding to -pi for the alpha and beta angles and bottom right being +pi.
int i,j;
for(i = 80; i >= 0; i--)
{
for(j = 0; j < 81; j++)
{
if(map[i][j].allowed == 1)
{
printf(".");
}
else
{
printf("#");
}
}
printf("\n");
}
}
int main()
{

struct configInfo myMap[81][81];
fillMap(myMap);

printMap(myMap);
//int x = getJoint2XLocation(-PI/4);
//int y = getJoint2YLocation(-PI/4);

//crossLine(-PI/8, -3*PI/8);

//printf("X: %d", x);
//printf("\nY: %d", y);

return 0;
}

```

http://castor.augsbu...p/1885/two.html

Is This A Good Question/Topic? 0

## Replies To: Dijkstra's Algorithm Implementation?

### #2 Oler1s

• D.I.C Lover

Reputation: 1397
• Posts: 3,884
• Joined: 04-June 09

## Re: Dijkstra's Algorithm Implementation?

Posted 19 April 2010 - 05:38 PM

I don't see a code issue anywhere. I see an algorithm issue. You need to solve a certain problem.

All we have here is problem + code. Are you expecting us to solve your homework for you?
Was This Post Helpful? 0

### #3 irmanbearpig

• New D.I.C Head

Reputation: 0
• Posts: 3
• Joined: 18-January 10

## Re: Dijkstra's Algorithm Implementation?

Posted 19 April 2010 - 06:21 PM

*edit* Sorry, this is guest_Lance, just figured I'd register while i wait lol...

Well I did write the whole thing, it wasn't code that was already given. I started the assignment with nothing so I'm not being lazy i promise! I don't want you to answer it for me, just give me some advice on how to ask it to go from one x,y coordinate to another without going into any space that is obstructed by the obstacle, I added the "allowed" variable in the configInfo structure to help with that... but that's where I got stuck and need suggestions!

Okay let me be a little more specific... I know I need to make a new function, call it
```void goToLoction(int x, int y)
{
//some form of dijkstras algorithm here to determine the shortest path
}

```

but I need to use Dijkstras algorithm in a way that actually moves the head from one place to the other. I figure just look through the whole graph using Dijkstra's algorithm and it will find me the shortest path, but I don't know how to turn that into something that will tell the head in specific (x,y) coordinates where to go... because there are some places that it cannot go if that position requires the arm to be bent in a way that would be going into the ground, through the arm, or through the obstacle in the way. I have go the program go through all possible places the arm could go and added in an attribute called "allowed" in the configInfo structure which is 1 if it can possibly go there and 0 if it can't... which will be helpful to determine the shortest path without going somewhere that would break the arm. But I can't wrap my head around how to tell it to go to the destination. Will I have to go through and make some kind of array and store a series of allowed nodes that it has to visit to eventually make it to the destination?

This post has been edited by irmanbearpig: 19 April 2010 - 07:06 PM

Was This Post Helpful? 0

### #4 Oler1s

• D.I.C Lover

Reputation: 1397
• Posts: 3,884
• Joined: 04-June 09

## Re: Dijkstra's Algorithm Implementation?

Posted 19 April 2010 - 07:06 PM

So, based on your problem description, I envision a problem as shown in the illustration attached.

In the illustration, center of head of some size must be placed at certain point without colliding with obstacle. To do so, the arm angles can be manipulated. Can you illustrate what the problem actually looks like?

#### Attached image(s)

Was This Post Helpful? 0

### #5 irmanbearpig

• New D.I.C Head

Reputation: 0
• Posts: 3
• Joined: 18-January 10

## Re: Dijkstra's Algorithm Implementation?

Posted 19 April 2010 - 07:10 PM

http://castor.augsbu...p/1885/two.html

This is a description of the whole problem, I had it posted at the bottle of where the code box was... realized it was hard to find lol.
Was This Post Helpful? 0

### #6 Oler1s

• D.I.C Lover

Reputation: 1397
• Posts: 3,884
• Joined: 04-June 09

## Re: Dijkstra's Algorithm Implementation?

Posted 19 April 2010 - 07:20 PM

The link is broken.
Was This Post Helpful? 0

### #7 irmanbearpig

• New D.I.C Head

Reputation: 0
• Posts: 3
• Joined: 18-January 10

## Re: Dijkstra's Algorithm Implementation?

Posted 19 April 2010 - 08:24 PM

This is the exact assignment description:

The third assignment consists of remapping your configuration space, and moving through it.

Our robot arm is still the same as in Assignment Zero, but we're now going to add an obstacle to our factory floor (in x-y space). Here's the layout: starting at x = 100 cm and y = 150 cm there is an obstruction stretching up to the ceiling. At position x = 120 cm and y = 180 cm there is an object to be grasped. It needs to be placed on a shelf at x = - 50 cm and y = 50 cm.

Your job is to move the object from its current location to the shelf. You must map how the robot arm will traverse from its start location (totally vertical) to the object, and then to the shelf.

As before, assume that the arm joints may not flex more than from -7/8 pi to +7/8 pi. Additionally, the arm is not allowed to cross into itself or the floor or the hanging obstacle.

Design, write, compile and run a program using the C language which prints out the map of the configuration space (it's 80 wide so it will print out nicely in 80 columns!) with the obstacle in it. Then design and run a program that will calculate the best path from the start position to the object and then to the shelf. Use, for example, Dijkstra's algorithm. Print out the path in both configuration and x-y space.

Was This Post Helpful? 0

### #8 Oler1s

• D.I.C Lover

Reputation: 1397
• Posts: 3,884
• Joined: 04-June 09

## Re: Dijkstra's Algorithm Implementation?

Posted 19 April 2010 - 08:44 PM

Are you not sure how to start?

How would you represent a certain robot arm position as data?
Was This Post Helpful? 0

Page 1 of 1

 .related ul { list-style-type: circle; font-size: 12px; font-weight: bold; } .related li { margin-bottom: 5px; background-position: left 7px !important; margin-left: -35px; } .related h2 { font-size: 18px; font-weight: bold; } .related a { color: blue; }