Genserkins 6-Axis strange behavior

More
16 Nov 2021 11:28 - 16 Nov 2021 11:30 #226645 by xcp92
 Hello,
i have a 6-Axis Robot arm (Motoman K10S) running with genserkins. Kartesian movement is working good if the last joint is pointing straight down.
If the last joint points horizontal the movement in the Y-direction is off ( to long and a slight ark).
If i use a pencil to track the movement and try to make a circle in the X-Y plane, I get a circle when the robot points straight down. When the robot is facing in the X - direction (horizontal) i get approximately an ellipse. (Pictures as reference).

It seems thougt the distance of the tooltip to the last joint is neglected in the movement, but the X - coordinate is correct. Y - Movement ( if Y near to 0 +- 200mm is off by 30%.  X-Movement is spot on. If i turn the robot 90° and X is near 0, Y-Movement is correct and X- Movement is off.

I followed the D-H Parameter tutorial and the Puma560 Demo helped a lot ( The robots are the simmilar in theconstruction and order of the joint - types, just the dimensions from joint to joint are different)
The DH-Parameter are attached.

This is strange or is this normal behavior??



Attachments:
Last edit: 16 Nov 2021 11:30 by xcp92.
The following user(s) said Thank You: DPFlex

Please Log in or Create an account to join the conversation.

More
17 Nov 2021 08:10 #226705 by Aciera
I'm surprised the usual genserkins setup works for your robot with that lever mechanism. However, since you have an issue with 'sideways' motion, that doesn't seem to be causing your problem. Have you tried increasing your D-5 value to check if the offsets gets smaller?

Are you sure your X-coordinate is correct when the wrist is horizontal as in the hand-drawn sketch on the left? If it is then D-5 parameter would seem to be correct.
Also, if you go from the horizontal to the vertical orientation does the z-coordinate value change by the value of D-5?

Looking at your parameter list (D-0 = 0), it seems that your origin is in your shoulder joint rather than on the base plate. Is that intended?
linuxcnc.org/docs/html/motion/dh-parameters.html

Note: If you are using the 'switchable kinematic' feature you should update to current master as a bug has been identified and a fix has been pushed recently.
The following user(s) said Thank You: DPFlex

Please Log in or Create an account to join the conversation.

More
17 Nov 2021 09:18 #226713 by xcp92
I got the lever mechanism working with a little software "hack", (Custom component that moves the second joint when the first moves, so that the net movement is only on joint 1, works flawlessly so far.) when the robot is fully working i will post my solution here.

If i increase the d-5 Offset to 400 or 0 not much changes. 

Are you sure your X-coordinate is correct when the wrist is horizontal as in the hand-drawn sketch on the left? If it is then D-5 parameter would seem to be correct.
Also, if you go from the horizontal to the vertical orientation does the z-coordinate value change by the value of D-5?

1. Yes i tested it multiple times.
2. I only measured the x - Value.  I will test the z - Value and then reply.

Looking at your parameter list (D-0 = 0), it seems that your origin is in your shoulder joint rather than on the base plate. Is that intended?

Yes its intended, this way, the z coordinate absolute 0 is near the Workbench surface. In absolute World mode its simpler to measure z height for me.

Note: If you are using the 'switchable kinematic' feature you should update to current master as a bug has been identified and a fix has been pushed recently.

As far as i know i dont use switchable kinematics. What is this feature?

Please Log in or Create an account to join the conversation.

More
17 Nov 2021 09:40 #226717 by Aciera
Also what you will want to check is the change in Y-coordinate value when you twist the arm to get the hand from vertical to a 'sideways' horizontal orientation (ie it should go from 0 to +/- D-5 value).

The 'switchable kinematics' feature let's you switch between different kinematic models in GCode. For robots it is often useful to switch between cartesian 'world' mode and trivial 'joint' mode because the mathematical modelling for the inverse kinematic fails in certain regions of the work space (singular points, gimbal lock). This is not unique to 'genserkins' but is a problem that all robot controllers have.
The following user(s) said Thank You: tommylight, xcp92, DPFlex

Please Log in or Create an account to join the conversation.

More
18 Nov 2021 20:53 #226918 by xcp92
Thanks for the help.
I found the problem when I turned joint 3 +90° and then joint 4 90°. The y-coordinate should be 195mm but it was -195mm. 
The other directions were correct. 
I switched the turning direction of joint 3 and 5. Now it works. 
Sometimes you can't see the obvious and need a hint :-D
​​​​​
 

Please Log in or Create an account to join the conversation.

More
19 Nov 2021 07:17 #226957 by Aciera
Nice!
Looks like you're planning to use it for welding, would be very interesting to see how that works out for you and please do post your custom component for the joint compensation.

Please Log in or Create an account to join the conversation.

Time to create page: 0.150 seconds
Powered by Kunena Forum