Discussion, proposals and status information is published here: https://forum.duet3d.com/topic/17421/rob...

The kinematics will be pull requested into RRF3, but while developing and testing it is created in https://github.com/JoergS5/RepRapFirmwar... as fork of RRF3 v3.02 dev branch. Filenames are FiveAxisRobotKinematics.cpp and .h

For compilation, a few additional source changes must be made according to

https://github.com/Duet3D/RepRapFirmware...

Status summary:

• done: documentation, implemented P0, P2, P3, P4, M208, R
• current: working on 3D printer and validate code, A paramter instead of M208
• open: bending and rail support, coding P1, D

Status History:

• Nov 23: still working on prototype, but will be finished soon. Then next steps are validate current firmware and implement missing parameters (P1, bending, rail support)
• Oct 18: added endstop positions to A parameter
• Oct 16: reintroduced A angles, new D parameter, not using M208
• Oct 07: reintroduced P1
• Sep 26: remove unused parameters D, P1
• Sep 24: M208 bugfix, started homing wiki
• Sep 22: M208 as angle limits, remove A parameter
• Sep 22: P3, P4 implemented
• Sep 20: Xo, Yo Axis 5 offsets implemented
• Sep 15: changed B parameter
• Sep 10: added A to M669 report
• Sep 08: separate rail properties (new C parameter), compact M669 status display

The robotic is construction like that (tbd better images)

Angles and Axes

• axis 1 is a vertical axis and is first actuator
• angle 1 is the angle of axis 1 in the horizontal plane, positive is counterclockwise viewed from above. 0 degree is when arms 2 and 3 are parallel to the x axis and the position is left (see image 2).
• axis 2 is a horizontal axis in Y direction. The xyz coordinates are when axis 1 is rotated so arm 2 and arm 3 are parallel to the x axis (viewed from above, see second image).
• angle 2 is the angle from horizontal to arm2 (arm 2 length L2). Upwards is positive.
• axis 3 is a horizontal axis in Y direction
• angle 3 is the angle between arm 2 and arm 3. Downwards (i.e. clockwise) is negative. Angle 3 is always 0 or negative.
• arm 3 length is L3.
• axis 4 is a horizontal axis in Y direction and rotated such a way, that arm 4 is vertical.
• angle 4 is the angle between arm 3 and 4. Down is negative. Angle 4 can be calculated from angle 2 and 3 (angle 4 = -90 - angle2 - angle3).
• arm 4 length is L4.
• axis 5 is vertical and the hotend holder is horziontal.
• angle 5 is the angle between axis 5 and the nozzle middle in the horizontal plane. Positive is counterclockwise viewed from above.
• arm 5 is the connection between axis 5 and the middle of nozzle and has length L5.

View from top:

This shows that arms 2 to 3 are in one line, arm 4 is vertical, arm 5 rotates horizontically.

An optional 6th rail axis is installed parallel to X, Y or Z axis and has drive letter W.

K13 is the printer type mode. Mandatory parameter without default.

Xm:n[:o] Ym:n[:o] Zn are positions of Axis 1 (m) and Axis 2 (n), both in absolute cartesian coordinates. The values are measured when axis 1 is rotated into a position when arms 2 and 3 are parallel to the X axis and from left to right (as in the second introduction image). Mandatory parameter without default. If second Y is 0, arm 2 will go through axis 1.

• Xm, Ym are the cartesian coordinates of axis 1. Z is not relevant
• Xn, Yn, Zn are the cartesian coorindates of axis 2
• Xo, Yo optional offset between end of arm 4 and axis 5, default is 0 each. See section Axis5offset section below for details.

Aa1min:a1es:a1max:a2min:a2es:a2max:a3min:a3es:a3max:a4min:a4es:a4max:a5min:a5es:a5max[:railmin:railes:railmax] define the allowed angles in degrees. rail.. is in mm.

a1 are the angles of axis 1 etc.

min ist the minimum angle

es is the endstop position (needs not to be between min and max, could be outside)

max is the maximum angle

Default is A-45:0:45:0:0:70:-70:0:0:-170:-90:0:-225:0:225[:0:0:1000]

Drad1:sph1:rad2:sph2:rad3:sph3:rad4:sph4:rad5:sph5 define the outline of the arms and axis actuators to calculate collisions.

The idea is to define cylinders around the arm and spheres around the actuators to define an area which collides if reached.

Cm:xxx:yyy:zzz are parameters if the rail is used

m is the mode of the rail, XYZ are the offsets from the coordinates. All four parameters must be used, or C0 without xyz. Invalid settings are ignored.

• C0 means no rail used. Default
• C1:x:y:z means parallel to X axis, with X offset
• C2:x:y:z means parallel to Y axis, with Y offset
• C3:x:y:z means parallel to Z axis, with Z offset. That means the robot moves up and down, e. g. if assembled at a wall.

Rn is the actuator construction type.

• R0 are 5 rotation actuators for axis 1 to 5. Driver letters are XYZUV. If rail is used, it is drive W. Default.
• R1 are 4 rotation actuators for axis 1 to 3 and 5. Axis 4 is rotated automatically by construction for arm 4 to stay vertical. Axis 1 to 3 are XYZ, Axis 5 is drive V. If rail is used, it is drive W.
• R2 are 3 rotating actuators for axis 1 to 3, driver letters XYZ. Axis 4 behaves like in R1. Axis 5 has no actuator and is fixed in 0 degree position. I rail is used, it is drive W. In this mode, G31 is only possible with probe at nozzle XY position.

Lnnn:mmm:ppp:qqq are the arm lengths for arms 2 to 5. Mandatory parameter without default. The arm length is measured from middle of hinge to middle of other hinge (arm 5: middle of nozzle).

Pn defines the behaviour of axis 5:

• P0 arm 5 is fixed to 0 degree (arm 2, 3 and 5 straight line when viewed from above). Default.
• P1 axis 5 can rotate freely inside its M208 limits. The strategy is to remain at the current angle as long as possible. When an unreachable area is reached, the angle is changed. If information of the planned moving path is available, this information is used to avoid jerky behaviour.
• P2:nnn arm 5 stays in a specific angle position nnn in respect to cartesian coordinates. If nnn is 0, it stays parallel to X axis, to be used for G31 mesh compensation. P2 can be used for tool changing.
• P3 arm5 is directed into the movement direction in respect to x and y coordinates (no Z tilting). For moves from right to left axis 5 must be able to be below -180 and above 180 degree, because it must correct axis 1 angles. So A the last pair of values should be in the range of -225:225 and the hotend assembly (inclusive wires and filament) must support it.
• P4 arm 5 is directed into the movement direction like P3, but for moves from right to left the direction is rotated by 180 degree, so axis 5 degree is between -90 and 90 degree most of the time. This can make sense if the extruder has boundaries which can be used bidirectional (e.g. concrete printer which has boundaries left and right of the movement direction).

Snnn Segments per second (because smooth XYZ motion is approximated by means of segmentation)

Tnnn Minimum segment length (mm) (because smooth XYZ motion is approximated by means of segmentation)

Bor:b2:b3:b4:b5 robot orientation and correction factor for bending the arms. Default is B0:0:0:0:0

Important: The coordinate system remains with X being the direction from axis 1 - arm2 - arm3 etc., Y "behind axis 1" and Z "above" the construction, even when the robot is assembled top-down (then Z directs to bottom). The B orientation tells the kinematics in which direction the arms bend.

or is the orientation of the robot in space

• 0 is like described above and default: in homing position arms 2 and 3 are horizontal and axis 1 is vertical.
• 1 in homing position arms 2 and 3 are oriented vertical to the space
• 2 in homing position arms 2 and 3 are like default, but axis 1 is rotated 90° (laying "on the side")
• 3 also laying on the side, but -90° on the other side
• 4 robot hanging at the ceiling, orientation like in mode 0, but upside-down.

I'l make images to explain when I have implemented it. The bending differs depending on the orientation due to the direction of the gravitational forces.

b2 to b5 is the bending for arms 2 to 5 in mm in Z direction at the end of each arm when bending of the arms is horzontal and maximal. If the arm is angled up or down, a fraction of the bending is calculated according to the forces. The parameter is optional with default value 0. Bending can be measured or calculated by e-Module and weight. The e-Module depends on the material of the arm. Bending and arm length shortage of the arms are approximated (otherwise calculations would be very complex), so bending to arm length ratio should be small.

Axis 1 to 5 are using the drive letters X, Y, Z, U and V. The rail version uses W for the rail movement.

M92 is calculated as usual, but instead of mm the degrees of the angles are used. Microsteps and Gear ratios must be accounted for in the values.

Example: Stepper with 200 Steps, 16 microsteps, Gear Ration 1:27: 200*16/360*27 = 240

M208 is ignored. The reachable world coordinates differ, depending on the print height, so it is better and exact to define the angles as limits (parameter A).

G31 X Y Z probe as offset relative to hotend position for mesh compensation.

G92, M114 the XYZ values are cartesian coordinates, the UV are angles. Axis 1 to 3 angles cannot be set with G92 directly.

A homing wiki is here: Homing a FiveAxisRobot

The current M669 setting can be asked for by calling M669 without parameters. Every time M669 is called to set all or some new values, the new settings are displayed. Float variables are displayed abbreviated to one digit after the dot, but internally they are stored completely.

The Object Model is defined including some of the important parameters like axis 1 and 2 coordinates, arm lengths, restrictions and current P mode.

M669 K13 X20.0:5.0 Y0.0:10.0 Z50.0 L300.0:400.0:200.0:100.0 R1 P2:45.0 C0

The example means: axis 1 is at position 20/0, Axis 2 is left of it and a bit behind and 50 mm height, the 4th axis has no actuator (R1), axis 5 has always angle 45° with respect to the XY coordinates and no rail is used.

As long as the source is developed in a fork, it needs to add information at three places.

In Kinematics.cpp:

#include "FiveAxisRobotKinematics.h"

then in the switch statement:

case KinematicsType::robot5axis:

return new FiveAxisRobotKinematics();

and in Kinematics.h in the enum of KinematicsType robot5axis.

In the .h it must be at the correct position:

...

markForged,

collinearTriperon, // reserved...

robot5axis,

unknown // this one must be last!

};

The code will compile without errors, if the projects tags 3.1.0 are used with the addition of the two Robot files. (Go to github into the project, click on the button Tags, download the 3.1.0 zip file). See https://github.com/Duet3D/RepRapFirmware... plus the changes above.

The offset allows that axis 5 doesn't need to be vertical below arm 4, but with an offset. In the image offset x is positive and offset y is negative:

M669 A sets the limits for axes and rail angles:

• axis 1 = X: 0 degree is straight to left. Positive is counterclockwise.
• axis 2 = Y: positive angle is arm2 up, negative down. Angle2 is the angle between horizontal and arm 2.
• axis 3 = Z: positive angle 3 is arm3 up, negative down. Angle 3 is the angle between angle 2 and angle 3.
• axis 4 = U: angle between arm 3 and arm 4, negative is down. If axis 4 angles are too restrictive, printing can be disabled, because arm 4 must be able to be oriented vertical.
• axis 5 = V: angle from straight line between arms 2 to 4 and arm 5. 0 degree is straight to right. Positive is counterclockwise. The angle can be smaller than -180 degree and bigger than 180 degree to allow some necessary axis 5 rotations in modes P1 and P3. Care with the wires and filament must be taken to use it.
• rail = W: coordinate position in mm.

An example of A setting is:

A-45.0:45.0:0.0:75.0:-75.0:-5.0:-170.0:170.0:-225.0:225.0