6 DOF robotic arm by Jos

[Mr Jos]

Hey,

EDIT: Last video will be shown here, rest is orignal post in order of progress.

V3 XL 6DoF robotic arm. Homing all 6 joints + demo linear movements + roll/pitch/yaw control.

 

V2 with more power and PS4 control for teaching new waypoints. Straight lines are calculated whilst moving from 1 waypoint to another, no saved coördinate lists except each waypoint is saved as (X, Y, Z, Roll, Pitch, Yaw) for the end effector position at that waypoint. It does not mather how you moved with the controller between these points.

Available at Rebrickable now:

https://rebrickable.com/mocs/MOC-76666/Mr_Jos/6-degrees-of-freedom-6dof-fully-automatic-robotic-arm-with-inverse-kinematics-programmed/#details

V1 6DoF

X/Y/Z Positioning with fully adjustable Roll+Pitch+Yaw possible. Angles are calculated on the brick with Inverse Kinematics and Forward Kinematics to check real current position, there are no lists being used with preset angles, only for calibrating all 6 joints.

End of EDIT___________________________________________________________________________________________________

 

 

I wanted to try to build and program a 6 DOF arm. Upper arm is based on photo's from akiyuki, lower part and base MOC. Now I want to try to write the program for it myselve completely with micropython. If that all works out well, I'll be looking into putting a pneumatic hand or something on it. All 360° functions can rotate infinite.

 

Need to do something whilst sitting out 7days quarantaine..

Edited February 2, 2022 by Mr Jos
New top video

[Jonas]

Keeping my fingers crossed for you!

I am working on a very similar project, also inspired by Akiyuki's arm. As to the 360 deg unlimited rotation: It looks fine but for a practical application I had to add stops to each of the rotating mechanisms in order to define start positions needed by a control program.

It seems that I have already managed the 2-EV3-brick control via Micropython, thanks to your initial advice.

Jonas

[Mr Jos]

I managed to initiate a homing for all 6 axis whilst still having the 360° unlimited rotation on all 3 those axis. Just needs some fine tuning on the 6th one tomorrow and then can the real programming start. A video can be found on Facebook, as it's easy to upload my not-so-good-unfinished-project-videos. And keeping finished ones on YouTube.

[Mr Jos]

Initiating all 6 axis, including the 3 infinite axis. Then performing some basic movements with point to point (calculating speed for each axis to arrive at same time). Better movements [linear along the field] will be programmed in the rest of my quarantine.

[Jonas]

Nice! A lot of inspiration for me.

Enjoy your quarantine and happy Christmas, too.

Edited December 24, 2020 by Jonas

[Newest_Tech320]

Very Cool

[Mr Jos]

After manyyy.... hours of writing, forward kinematics are calculated manually finally. Took some long reading, understanding and checking diagrams. But it does seem to work to find X/Y/Z position for any degrees of the joints.

Now trying to find out how I can get the inverse kinematics done..

[Mr Jos]

I put in the calculations for X/Y/Z position of the fork position based on the angles from the motors.

X and Y work perfect (shows distance in mm from center of base.) Z axis has somewhere a + and - (or more then 1) misplaced I think  doesn't work as intended yet.

The program still works fine, but had to slow down bluetooth refresh rates a bit because the continuous calculations asked to much of the EV3.

Somewhere in here is a - misplaced, do you see it? 

[Mr Jos]

Finally an update in the controlling of the 6DoF.

The only input for moving in a straigth line from current position to new position, XYZ is in millimeters from center of robot and floor level = (Xnew, Ynew, Znew, Roll, Pitch, Yaw, Maxspeed)

while True:

    next_coordinate(160, 200, 380, 0, 0, 0, 500)

    next_coordinate(160, 200, 140, 0, 0, 0, 500)

    next_coordinate(360, 0, 140, 0, 0, 0, 500)

    next_coordinate(160, -200, 140, 0, 0, 0, 500)

    next_coordinate(160, -200, 380, 0, 0, 0, 500)

    next_coordinate(300, 0, 200, 0, 0, 0, 500)

    next_coordinate(160, -200, 380, 0, 0, 0, 500)

I finally made a working code to do basic inverse kinematics. I need to test and make it better/be able to do more. For now it will work for in θ1 = [90° -> -90°] (as it keeps the end effector always pointing in the same direction as roll/pitch/yaw needs to be 0). θ1, θ2 and θ3 are calculated by finding the wrist X/Y/Z. Then θ4, θ5 and θ6 are calculated to keep the end effector in same direction and flat.

The movement is already smoother now than in this video as I changed some parameters and it doesn't drift so far anymore. Now I want to try adding Yaw to the end effector (keep it flat but turn around) so I can use the full 360° of θ1.

Note that it already uses full 360°+ rotation on θ4 and θ6 when it keeps making rotations around.

 

 

[Jonas]

Good job, Jos! I am glad that you did not give up.

Two questions:

1. When you say the end effector is moving in a straight line, do you mean point-by-point straight line? In other words, do you run incremental control?

2. How difficult was to incorporate the compensation of the mechanical links ( θ4, θ5 and θ6) into the IK equations?

 

Just FYI: In my own project, I am pausing now. I managed to program and run 4DoF IK control. It runs well when you move the arm in free space but when I wanted to apply it for a real task (a sort of chess figures moving), a lower precission of positioning became critical. That is why I am trying to build a smaller and more compact robot, now.

 

[Mr Jos]

I programmed it so that it pulls the current X/Y/Z position from the forward kinematics, then with the new coordinates it knows how far it should move the end-effector in X/Y/Z. I pull out the largest of the 3 and make it move 6mm at a time closer to the new position. As I know howmany steps of 6mm I will need to get there I divide the other 2 coordinates in equal steps and move a 'step'. This is calculated quickly and each position is send to the move subroutine, by calculating the time needed to arrive at the given position I know when to send the next position. By finetuning this I managed to make it move smoother so it does not jerk incase it stops at each position.  θ1, θ2 and θ3 are no problem at all, even with a roll/pitch/yaw trim. But  θ4, θ5 and θ6 now only are calculated correctly if end-effector is at 0° roll/pitch/yaw. At this moment trying to find out what I need to change in the code for a 90° yaw move and if all positions in between will be correct.

As testing used up much time homing all joints I tried speeding it up, and it seems to stay relativly stable so far.

If I find anything more for the IK I will post it here.

[Skookumjim]

Well done, ...an inspirational project.

My very first attempts with Robot Inventor was to make a very simple robot arm... https://www.dropbox.com/s/cv5cr4liu08afcr/IMG_9828.mov?dl=0

I have to say that the ungeared results were not very encouraging in that the motors proved not to up to be up to the job... both in the context of holding position under load or moving precisely... a much larger geared solution seems more promising.

[Mr Jos]

20 minutes ago, Skookumjim said:

Well done, ...an inspirational project.

My very first attempts with Robot Inventor was to make a very simple robot arm... https://www.dropbox.com/s/cv5cr4liu08afcr/IMG_9828.mov?dl=0

I have to say that the ungeared results were not very encouraging in that the motors proved not to up to be up to the job... both in the context of holding position under load or moving precisely... a much larger geared solution seems more promising.

Thanks,

These motors do need some gearing down indeed to be accurate and powerfull enough. And I've seen in a video that the power from the RI motors is lower then the EV3 Large motors so it would need even more gearing down

pitch_base_full_rot = 3600   # 360 /  12 * 60  /  12 * 24

pitch_arm_full_rot = 4500    # 360 /  12 * 60  /  12 * 24  /  16 * 20

roll_arm_full_rot = 1800     # 360 /  12 * 60  /  16 * 16  /  16 * 16

yaw_arm_full_rot = 3600      # 360 /  12 * 60  /  12 * 12  /  16 * 16  /  12 * 24

roll_head_full_rot = 2520    # 360 /   8 * 56  /  12 * 12  /   4 * 4 

yaw_base_full_rot = 4200     # 360 /  12 *140  /  12 * 12

That's the gearing I'm running now. If I would start again with this robot I would gear down the base pitch more, as now it's sometimes not strong enough when fully extended. 3600 means it needs 10 full rotations of the motor to turn the joint 1 rotation, or 1motor degree turning = 0,1° on the robot.

 

Edited January 27, 2021 by Mr Jos

[Mr Jos]

Just had my first Yaw adjustment working, Roll also works well. It kept the fork flat and making a 90° turn whilst going to the new X/Y/Z point. Will do some more testing, and if all work correct I might make a new video. If so then I only need to add in pitch (not needed for a robot acting as forklift as it should always stay flat as it is now.

The looptime did not increase much with the yaw movement added, still within 52-89ms to find new thetas and corresponding speed values for those joints.

Edited January 28, 2021 by Mr Jos

[Mr Jos]

And here is the promised video,

For the people who want to know how I'm doing the IK so far in the program (Don't have pitch yet), I show it in the second half of the video.

Full control to make the end-effector go to a X/Y/Z position, now the input is a hard-coded list of end-positions (no lists are used to know the angles for every position, it gets calculated), but you can use anything if you want, remote control to add extra X or Y or Z or all together. Meanwhile it can change the end-effector's Roll and Yaw (Roll not shown in video so it's easy to see the fork stays firmly upright, but Roll is easy, just add Theta6 = Theta6 + Roll. As it's not affecting the position.

In the video I also show the code for this robot, remember, I started from 0. I did not use any code from someone else, and I started programming just few months ago for the first time. So it's maybe a mess and some things to long. Feel free to give feedback how to make it 'faster' (meaning lower looptime = can make steps smaller than 6mm).

Edited January 29, 2021 by Mr Jos

[Jonas]

Your progress is amazing. Moreover, you have proved that even a large project with rather complex math can be implemented in Pybricks.

[Jonas]

I have another question: How do you handle multiple solutions to the IK task? Or do you just take the first one?

[Mr Jos]

25 minutes ago, Jonas said:

I have another question: How do you handle multiple solutions to the IK task? Or do you just take the first one?

Thanks, I have been writing down all kind of possibilities for combinations of positive for this, negative of other, but smaller or bigger, etcetc and then tried finding calculations that work. And each time track back to see if it still works for other components.

This is/was very time consuming finding the correct equations to eliminate the multiple solutions like for θ6; (Carefull this is not correct yet, still trying to find solution working for pitch)

if theta32 - pitch > 0:

        theta6 = math.degrees(math.asin(-math.cos(math.radians(theta32 - pitch)) * math.sin(math.radians(theta1 - math.fmod(yaw, 360))) / math.sin(math.radians(-theta5)))) + roll

    elif theta32 - pitch < 0:

        theta6 = math.degrees(math.asin(-math.cos(math.radians(theta32 - pitch)) * math.sin(math.radians(theta1 - math.fmod(yaw, 360))) / math.sin(math.radians(theta5)))) + 180 + roll

    elif theta1 - math.fmod(yaw, 360) < 0:

        if math.fmod(yaw, 360) == 0:

            theta6 = 270 + roll

        elif math.fmod(yaw, 360) != 0:

            theta6 = 90 + roll

    elif theta1 - math.fmod(yaw, 360) > 0:

        if math.fmod(yaw, 360) == 0:

            theta6 = 90 + roll

        elif math.fmod(yaw, 360) != 0:

            theta6 = 270 + roll

So there's already 6 possibilities for my θ6, but it will only choose the one fitting current 'stance' of the robot.

Just picking one will make the joints go nuts and do random 180° turns. After the video I had to change a bit of code to allow unlimited turns for θ1, it works as long as pitch stays within 360°, might have to take a look at it after I finish the pitch.

I did have LOADS of time, and it was needed to program the IK, as I'm in a quarantaine hotel for 15nights (you can see it in the video). And can't leave my room for 1minute. 3days left now... tic tac...

 

[Jonas]

Thanks for the explanation. I was also wondering how you managed to find almost 'unlimited' time to work on your project. Now I understand. I could work on mine during Xmas holidays but now it is much harder to find some longer time as any interrupt slows me down.

[Mr Jos]

I'm pretty sure I just found the answer for θ5. For every case using any [Roll, Pitch, Yaw] matching any angles [θ1, θ32] that I tried it seems a logical result for me. After θ1, θ2, θ3 that are easy, θ5 is actually just a very short but easy calculation with no need of an equation, but if you don't have any documentation like me it's tough.

Tomorrow I start searching θ4 and maybe θ6. If I do find it I will clean up my program a bit and show to those who are interested, or just give some hints how to do it.

Here is what I got so far, color code is in the screenshot.

Edit: θ4 and θ5 seem to be working good now with pitch in a few tests (but without yaw/roll mixed) as my θ6 is still not good for pitch, I get Math Domain error whilst I did not expect it.

Edited January 30, 2021 by Mr Jos
θ4 and θ5 found for pitch

[Mr Jos]

Quick update: It's alive! Roll/pitch/yaw of end-effector is fully functioning. I can make it take any stance, and fluidly transition to the next stance whilst moving to new X/Y/Z coordinates. Only thing is that it sometimes does a 360° on the θ6 whilst unwanted, need to look how I can solve that, but the biggest part is finished. Thought it would never be possible with Micropython.

I also need to change the path finding as it takes XYZnew - XYZold and moves 6mm at a time to the largest of those 3 distances. But now that I can roll/pitch/yaw around 1 XYZ point I need to look into moving certain ° at a time if the XYZ distance is to small (or 0).

Once that all is finished I will take another video and those who want to make their own 6DoF move with IK can try to see in my messy program how I did it.

To do:

- Remove unwanted 360° turns on θ1, θ4 and θ6.

- Change pathfinding for short distances, but high roll/pitch/yaw change.

- Cleanup program.

- Make a video

 

EDIT: 360° Unwanted solved!

Edited January 30, 2021 by Mr Jos

[Mr Jos]

Not perfect, but now it's only looking to optimize it as much as possible to try smooth it out more. Now we know 6DoF IK is possible with Pybricks!

[Jonas]

I can just repeat what I have already written several times in this thread: Very concentrated work and wonderful results!

In your video, I liked mainly the last task when you draw the contour of the square baseplate at zero height. A master piece.

Although I noticed that you had also some visible (1-2 studs) deviations, namely in the corners. I suppose it is due to the mechanical limitations of the plastic material.

I wonder what will be your next big challenge.

[Mr Jos]

11 hours ago, Jonas said:

I can just repeat what I have already written several times in this thread: Very concentrated work and wonderful results!

In your video, I liked mainly the last task when you draw the contour of the square baseplate at zero height. A master piece.

Although I noticed that you had also some visible (1-2 studs) deviations, namely in the corners. I suppose it is due to the mechanical limitations of the plastic material.

I wonder what will be your next big challenge.

The deviation was from me being impatient to upload a video. The zero-ing I use for theta1 is still the same as months ago, and that was never checked if it actually is correct, it was on sight done and it's off by a few degrees as I also changed the support under joint 1, the homing isn't 0°. Maybe it's better I look to control it sooner then later haha.

If you take in mind that theta1 is a little shifted it is a nice square.

Thanks for your comments and feedback, next thing will be to try control it with a BT remote, need to look at what is possible. So I can jog the robot to certain points and save them to perform several tasks without having to hardcode the positions/pitch/yaw/roll. But as I'm far away from home I don't know when that will be.

 

EDIT: I did find the correct value for positioning theta1. It was off by 22motor degrees (4200 for full rot = 2°) Now redid the square around base and it's waaay better.

Edited January 31, 2021 by Mr Jos
Found correction for theta1

[Skookumjim]

So... inspired by the opening photographs in this thread I have built such a large robot arm using Spike Prime motors... will post a photo once I have a decent one to share... meantime a few learnings for those who might be interested (given that this is the most complicated technic model I have attempted to build from scratch I might be teaching granny to suck eggs!)

1 Spike Prime motors need to be geared down to move weighty loads... the highest gearing I have is 15:1. Much is 5:1

2 Every precaution needs to be taken to eliminate gear slip... the higher the load, the greater the risk. In extreme cases that may lead to opposing wheels driving a turntable. Lots of trial and error here!

3 The through holes on the large motors are very helpful in building up gearing

4 The hub is best placed off a moving arm ... avoids undue torque loading

5 A single hub is barely enough.... 6 DOF plus some sort of action at the business end requires more than 6 ports... 

6 short cable lengths are limiting 

7 some head bending gearing calcs are needed to figure out are what speeds different motors are to run simultaneously to eliminate gearing interference between different limbs of the robot... for example without this a “wrist” rotation will cause the “hand” to move.

8 I still havent found a truly satisfactory solution for the business end... at present it just fires rubber bullets... and probably wont until it is possible to increase the number of ports and lengthen at least 1 cable

9 Much easier to play with code when cable connected... No dropping out of bluetooth that way. I havent delved into python but for the moment am happy enough with tge drag and drop software.