Help with Labview robotics algorithm

[shaazg]

Hi all,

I am new to labview and am currently working on a robotics project. I am having a small and relatively simple problem with labview and I think should only take 5-10 mins to fix but i unfortunately can not figure out what to do.

A brief introduction to the project, it is a robot snake that reacts (dances) to music. The snake robot is essentinally a serial robot, however the mechanism features independent (decoupled) joints; i.e. for most serial robots the forward kinematics may look something like this L1*cos (pheta 1)+L2 * cos(pheta 1+pheta 2); however for this mechaism all the angles are independant:  L1*cos (pheta 1)+L2 * cos(pheta 2). The forward kinematic equations can be seen in the attached VI and I am fairly certain they are correct.

The purpose of the VI is for the robot to move from an intial position (where all angles are 0) to a standing position (where all angles are 90) on its own. To do this an CCD (cyclic co-ordinate descent) inverse kinematics algorithm is used.  The program starts with the initialisation of all the joint angles as 0 and the forward kinematic equations to define the robot. The output of this formula node, is the x and y positions of the joints.The X, Y, and P (pheta) array is outputed to the CCD algorithm formula node, which computes two vectors for each joint , the vector from the joint to the end effector, and the vector from the joint to the end position. It then finds the angle between these two joints. In the second formula node is where my first problem comes up, currently in the vi it states i=5, i want it however to do a for loop of i goes from 5 to 0.

The output of the CCD inverse kinematics formula node is naturally the joint angles (p-array). These angles have to then be added to the original p array at the start of the program (so that the robot can stand up). When i try to sum the arrays essentially nothing happens, and all the numbers go blank and the robot will no longer move.  The last problem i have is that i need the entire vi to iterate untill the end effector (x6, y6) reaches the final position (xt, yt) this will probaly require around 100 iterations of the ccd formula node going from 5 to 0.  (Note the ccd algorithm is just a start to try and get the program to work, a more efficient algorithm is going to be implemented afterwards)

I will appreciate any help, if you have any questions please let me know.

Thank you

[david.bolt]

Dear Shaazg,

Please find an attached VI with a recommendation that could be used. Use a case structure to select between the different values of 'i'. Also when x=6 and y=6, the VI will stop using the simple code I implemented. Try use the case structure to select between different values. I hope this points you in the right direction. Many thanks.

[S_Kamath]

Hi,

Per the Help files the syntax for a "for loop" in a formula node is

for ( [assignment] ; [assignment] ; [assignment] ) statement

Therefore to loop over i=5 downto i=0 you would want something along the lines of 

for(i=5; i>=0; i--){

//your code here

}

 I am also not convinced that you want to add the new P array to the old P array. I am not familiar with the algorithm but it seems like you would like to replace the old P array with the new P array for the next iteration.

Also your algorithm was running at max speed and therefore coverging to its final value before it displayed the intermediate values. I would suggest a suitably large wait so you can see the intermediate values or can also consider using highlight execution and retain wire values to debug your code.

[shaazg]

Thanks for the suggestions and help.

Here is the final VI if anyone is interested. It shows the robot go from initial position (laying down) to final (upright) using the CCD algorithm. Unfortunatley it takes 1000s of iterations to converge, adjusting the wait time is good to see the first initial positions, but deleting it shows the solution converging.

[S_Kamath]

Hi,

Thanks for the VI, it is quite interesting. Are you trying to find the inverse kinematics of the upright position or are you trying to find a path that will get you from the prone to upright position? I thought you were trying the latter. If so you should realize that most of these convergent algorithms can benefit from a accurate starting point. So you should use the previous position as the starting point for the next iteration. So you may take a 1000 iterations for the very first Inverse Kinematic solution but subsequent solutions should be faster.

P.S.

If you are familiar with the Denavit-Hartenberg conventions and willing to work in them the you may be interested in the Robotic Arm API in the just released LabVIEW Robotics Module. It can help you calculate jacobians, forward kinematics, inverse kinematics etc.

[shaazg]

I am familiar with the D-H conventions and have begun playing with the new labview robotics module. However I do not know if I can use it. If you look at my forward kinematic equations you see that each joint is essentially independent of the one before it. You can see it in the attached VI. In conventional serial robots if you rotate the base joint the entire chain will rotate by the same amount. However with the mechanism I am modelling, if you rotate the base joint, only the base joint rotates and the others remain in the same angular position. Is it possible to use the new robot arm vi's in my case?

[S_Kamath]

It seems to be only a matter of convention, you should be able to convert between the two easily.

Assuming the normal convention is theta and your convention is p

p(0)=theta(0);

p(1)=theta(0)+theta(1);

p(2)=theta(0)+theta(1)+theta(2);

 etc.

Each joint in the serial arm affects the position of the rest of the arm, the only difference in your convention is that the zero reference is constant with respect to the world instead of constant with respect to the previous link.