Robot Kinematics Models

Introduction

This document was written in accordance to the Kinematics proposal document received as an additional development request for the softMC firmware. Following the proposal, this document is divided according to the development phases:

• PUMA Kinematics
• DELTA Kinematics
• Tool, base and compound operator

The three parts of the document will be incrementally added.

New kinematics models of PUMA and DELTA robots are made always keeping what has been implemented for SCARA robots. Therefore if not explicitly stated different, all robotics features that ware developed and implemented for the SCARA model will be available for the two models also. Therefore all the capabilities, methods and definitions of the features are common to all three models. The common robotics features for the above robots are:

• Straight-line motion
• Circular movements
• VIA (Pass-Through) movements.
• Continuous-Path – blending.
• Conveyor tracking (Moving Frames).

These features will be not described in this document.

Overview

This document describes a general kinematics implementation based on the softMC system version 2.1.6. In addition to the built in SCARA kinematics the following new kinematics models are added:

• PUMA kinematics of classical PUMA robots of 6 degrees of freedom.
• DELTA kinematics of DELTA parallel robots of 4 degrees of freedom.
• USER kinematics for general user-defined robot kinematics of up-to 6 DOF systems.

First a general description of coordinate systems and location definitions will be given for all types of robot kinematics. Each robot type will be described separately in the following order:

• PUMA robot specific will be given
• DELTA robot specifics will be given.
• General USER robot specific will be given.

At the end different types of user frames will be given (tool, world, base, machine, …).

Definitions

• DOF – “degrees of freedom” of a robot, usually a number of motors in the robot.

• DH – Denavit Hartenberg parameters of robot describing robot geometry set of (θ _i, α _i, a_i, d_i) numbers for each robot segment.

• “Tool-Center-Point” - point in the middle of the last robot segment. Initially in the center of the sixth segment flange if a gripper is mounted it coordinates are displaced by the tool property (will be described in phase 3).

• In the whole of the document we use term “point” for a vector describing position and orientation of the robot’s tool center. Depending on the robot type the point can be element of different vector spaces (two, three or six dimensional).

• A “location” is softMC data-type containing cartesian coordinates of a point.

• A “joint location” or “joint” is softMC data-type containing joint coordinates of a point.

Coordinates Systems

Cartesian coordinates and Euler Angles

In this section a method of robot point definition is shown. The presented method is given for a six-dimensional space defining both position and orientation, each having three degrees of freedom. For systems with more or less dimensions other coordinates systems will be used.

Each robot type is described with different group model. This is expressed in softMC Basic-Moves language in two domains. One is when the group is defined, and then the different model values are used. The other issue is the use of location or joint robot-type with a dedicated token values (XYZ, XYZR, …). Each robot (group) model has different robot type depending on the number of axes and the model value:

Robot:	robot-type token	Model value used in group definition:	Number of axes
SCARA	XYZR	4	4
PUMA	XYZYPR	2	6
DELTA	XYZR	6	4

Examples:

PUMA robot is defined by:

common shared puma as group axnm = a1 axnm = a2 axnm = a3 axnm = a4 axnm = a5 axnm = a6 model = 2

A location of PUMA robot is defined by: common shared lct as location of XYZYPR

DELTA robot is defined by:

common shared delta as group axnm = a1 axnm = a2 axnm = a3 axnm = a4 model = 6

A location of DELTA robot is defined by: common shared lct as location of XYZR

XYZYPR vs. XYZR

The XYZYPR token for PUMA stands for: X-Y-Z-Yaw-Pitch-Roll coordinates and the XYZR for SCARA and DELTA robots stands for X-Y-Z-Roll. In both cases the roll angle defines the same thing - it is the angle of rotation about Z-axis. This is to be remembered later when the compound operator is defined then the following relation is true:

X,Y,Z, ,Roll corresponds to X,Y,Z,Roll with Yaw & Pitch equals zero.

Cartesian coordinates describe robot locations defining position and orientation of the declared robot tool center point. A location can be expressed in at least three ways representing all 6 degrees of freedom encountered in the cartesian space:

• X Y Z Yaw Pitch Roll
• Homogenous transformation matrix
• X Y Z Quaternion

Common to all methods is the duality between location and transformation. Note that cartesian location gives coordinates of the tool center point which can be always interpreted as a transformation form the WORLD base point (space origin) to the tool center point.

XYZ Yaw Pitch Roll

This coordinates representation is mostly used in robotics. The softMC system uses it for user-related inputs and outputs (constant assignments and prints).

• 3 degrees of freedom in translation (X, Y, Z) expressed in mm, defining the position of the tool center point relative to the robot base.

• 3 degrees of freedom in rotation (yaw, pitch and roll defined below) expressed in degrees, defining the orientation of the tool tip. Yaw, Pitch, and Roll are the modified Euler angles and correspond to the sequential rotations around Z, Y, Z axes respectively.

• 

  Yaw definition
  Yaw is rotation around the Z-axis in relation to the previous frame. The previous frame is the starting coordinate system we are rotating. The figure illustrates a yaw rotation of 30°. Rotation about the Yaw angle yields new coordinate system (X₁, Y₁, Z₁).

• 

  Pitch definition Pitch is rotation around the Y-axis in relation to the previous frame after yaw rotation has been applied. It means the rotation is made around newly obtained Y₁ axis. The figure below illustrates a pitch rotation of 40°. Rotation about the Pitch angle yields new coordinate system (X₂, Y₂, Z₂).

• 

  Roll Definition Roll is rotation around the Z-axis in relation to the previous frame, after the yaw and pitch rotations have been applied. It means the Roll rotation is around Z₂ axis. The figure below illustrates a roll rotation of 20°. Rotation about the Roll angle yields new coordinate system (X₃, Y₃, Z₃).

	NOTE
	Note that the order of the yaw, pitch and roll sequence is important!

Homogenous Transformation Matrix:

This method of location representation is not used internally or in a user interface of the softMC but it is mostly used in the robotics literature. Therefore the representation is given in this document for demonstration and explanation purposes only. Location variable is represented as a (4 X 4) matrix:

loc =

Where: x_{x_axis}, y_{x_axis}, z_{x_axis} are normalized values defining the change of the x axis in relation to its initial position, etc ...

X, Y, Z are the position values of the tool center point relative to the robot base.

The last line (0 0 0 1) is used for normalization.

Example1 :

if loc coordinates are {100, 80, 150, 45, 0, 0}

loc written as a transformation matrix will be :

loc =

as there is only 45° of yaw rotation (rotation around Z axis) as it is illustrated.

The normal softMC system representation in programs is the {X,Y,Z, Yaw, Pitch, Roll} format. The homogenous matrix transformation is used here only for illustration purposes.

X Y Z Q (quaternions)

• 3 degrees of freedom in translation (X, Y, Z) expressed in mm, defining the position of the tool center point relative to the robot base.
• 3 degrees of freedom in rotation, expressed using the normalized quaternion Q described as a rotation around vector n for the angle phi: [cos(phi/2), sin(phi/2)*n]

The quaternion rotation presentation is used internally by the system but is not directly available for the user. However in cases of user-kinematics, this must be taken into account. The quaternions are used only for illustration purposes, the softMC system uses Euler angles for all user inputs and outputs.

Joint coordinates

For joint coordinates, a location variable is defined by n components representing the values in degrees (or mm) of each axis (n-number degrees of freedom according to the zero position of the kinematics model.

A location in joint coordinates is an array of n joint values:

Jnt_loc = {jnt1, jnt2, jnt3, jnt4, jnt5, jnt6}

Individual coordinates can be accessed via index definition in the form {i}: Jnt_loc{1}, Jnt_loc{2}, …

The above definition allows us to perform computation on locations in joint coordinates, for instance addition, subtraction, etc ...

Cartesian coordinates

For cartesian coordinates, a location variable is defined by 6 components representing the values: X, Y, Z, Yaw, Pitch, Roll.

Cartesian system variables: here, setpoint

loc = <robot>.here system maintained variable which returns the current transformation of the tool tip referred to the WORLD reference frame of the arm.

Cartesian location definition:

The curly brackets preceded by the "#" sign are used by the system to differentiate between joint and location positions.

#{x_value, y_value, z_value, yaw_value, pitch_value, roll_value}

Example1 :

If "r" is the radius of a circle and "angle" is the angle of rotation about the circle, then the transformation : #{r*COS(''angle''),'' r''*SIN(''angle''), 0, 0, 0, 0}

will yield points on that circle.

Point Manipulations

Compound operator

A compound operator between two cartesian points is defined. The compound operator is defined using the ':' sign.

Thus, a compound transformation is written:

location = loc1 : loc2

which means that loc2 is defined in relation to loc1.

A compound transformation is the result of the multiplication of the corresponding homogenous transformation matrix by another homogenous transformation matrix:

location =

Or using quaternions:

Example :

point_name3 = point_name1:point_name2

point_name3 is the compound transformation of point_name1 and point_name2 and is defined in the basic frame (WORLD).

Note that the sequence of a compound transformation has to be respected (operator is not commutative).

Example:

hop =#{20,130,45,30,0,90}:point_name

does not define the same transformation as:

hop = point_name:#{20,130,45,30,0,90}

Inverse of a location

Inverse transformations are of prime importance in robotics when manipulating trajectories in space. This notion is mainly used in relation to frames to move a trajectory into another place without re-teaching the points. The function which defines the inverse is called INVERSE.

The inverse of a transformation is the computation of the inverse of the transformation matrix.

INVERSE(loc) =

Or using quaternions:

INVERSE(loc) =

Example: benefit of inverse transformations

Example2 :

If "frame" is a transformation defining the position of the center of the circle and the plane in which it lies, the following program segment will move the robot toll point around the circle in steps of 1 degree.

FOR angle = 0 TO 360

MOVE frame:#{r*COS(angle*pi/180), r*SIN(angle*pi/180), 0, 0, 0, 0}

END

MOVE #{10,0,20,0,0,0):HERE:point = HERE is used as a location

Location assignments

loc =#{0,0,0,0,0,0}

A NULL transformation corresponds to the identity matrix.

NULL =

Example :

loc2 = loc1:loc

where loc is a NULL transformation

loc2 =

• loc2 is equal to loc1

Inverse transformations and NULL transformations

Direct coordinate transformation

The direct coordinate transformation translates joint variables (in degrees) into location variables (in mm) of the tool point center (modal value), referenced to the robot base (modal value). The system function used in this case is: <location> = ToCart(<robot>, <joint position>) It converts a set of joint coordinates to an equivalent transformation value using the geometric data of the robot given as a function argument. The computed location represents the position and orientation of the end of the tool in the World coordinate system taking into consideration the current TOOL transformation and BASE offset.

Application :

trans = ToCart (jtloc)

input : jtloc : joint

output: trans : computed location based on the given joint values of jtloc

See Also: TOCART

Example :

The series of instructions below computes the position and orientation that the robot will be moved to if its current location is altered by rotating joint 1 by 10 degrees.

jtloc = pfb

jtloc {1} = jtloc {1} +10

trans = ToCart(jtloc)

Inverse coordinate transformation

The inverse coordinate transformation translates robot locations into joint positions according to configuration flags and the modal tool & base values.

The system function used in this case is: <joint position> = ToJoint(<robot>, <location>, <cfg. Flags>) See Also: TOJOINT

{{Note | The conversion from cartesian into joint coordinates can result in several configuration solutions linked to a cartesian position. The desired configuration can be forced by altering configuration flags. }

The number and type of configuration flags depends on each robot type. The following table defines configuration flags of each supported robot type:

Robot type	Flag	Values		Bit No. (ToJoint)
1	2
PUMA	Arm	lefty	righty	0
Elbow	below	above	1
Wrist	noflip	flip	2
SCARA	Arm	lefty	righty	0
DELTA	-	-	-

The configuration flag argument of the tojoint function is defined as:

$cfg=\sum _{i=0,1,2}(value-1)\cdot 2^{i}$

Note! Constants lefty, righty, above, below, flip and noflip are defined as built-in constants of the MC-BASIC language.

Example:

In PUMA RX90 manipulator the following joint point: {0 , -45 , 135 , 0 , 90 , 0} corresponds to the Cartesian coordinates:

#{768.198 , 0 , 233.198 , 0 , 180 , 0}

and the following configuration: lefty, above, noflip. Therefore the configuration flags of the ToJoint function will be expressed as: (1-0)*2⁰ + (2-1)*2¹ + (1-1)*2² = 2. So the opposite direction will be:

-->?ToJoint(#{768.198 , 0 , 233.198 , 0 , 180 , 0},0b010)

{0 , -45 , 135 , 0 , 90 , 0}

-->

PUMA robot model

The PUMA robot is a six – degrees of freedom articulated robot of the following joints:

Coupling

In Staubli RX robot family there is a mechanical coupling between axes 5 and 6. The coupling matrix of the robot is defined by:

$C={\begin{bmatrix}1&0&0&0&0&0\\0&1&0&0&0&0\\0&0&1&0&0&0\\0&0&0&1&0&0\\0&0&0&0&1&0\\0&0&0&0&-1&1\\\end{bmatrix}}$

RX90 Work-Space

• 

  Puma work-space from above

• 

  Puma work-space from aside

• 

  Puma Lefty-Righty configurations

The PUMA WORLD workspace is limited by joint limits and the following auxiliary cartesian boundaries, available for the user:

• RMAX – maximum radius of a cartesian point (${\sqrt {(x^{2}+y^{2}+z^{2})}}$), no points beyond this radius are allowed.
• RMIN - smallest radius of a cartesian point (${\sqrt {(x^{2}+y^{2}+z^{2})}}$). No points inside this radius are allowed.
• ZMIN – z coordinate of the robot base plane. No points below this plane are allowed.

Robot configurations

Every robot location can be achieved in at least 8 different joint positions (the number can be greater counting also different joint positions obtained by changing joint angles for full revolutions (360 degrees)). Different joint coordinates of one location are called robot configurations. The PUMA robot configurations are defined by three flags: arm, elbow and wrist. Each flag has its command and feedback value: acmd/afbk, ecmd/efbk, wcmd/wfbk. Command values (acmd, ecmd, efbk) describe the given target point configuration of a movement (can be both nodal and modal). The feedback values (afbk, efbk, wfbk) represent the current robot configuration (pfb – values), and these are read-only flags.

The arm flag defines LEFTY or RIGHTY configurations according:

---

RIGHTY – positive jont-2 moves the wrist in positive WORLD Z direction while joint-3 is not activated

LEFTY– positive jont-2 moves the wrist in negative WORLD Z direction while joint-3 is not activated

The elbow flag defines ABOVE or BELOW configurations according:

---

ABOVE - position of the wrist of the ${\begin{Bmatrix}RIGHTY\\LEFTY\end{Bmatrix}}$ arm with respect to the shoulder coordinate system has ${\begin{Bmatrix}negative\\positive\end{Bmatrix}}$coordinate value along the Y axis of the second segment

BELOW - position of the wrist of the ${\begin{Bmatrix}RIGHTY\\LEFTY\end{Bmatrix}}$ arm with respect to the shoulder coordinate system has ${\begin{Bmatrix}positive\\negative\end{Bmatrix}}$coordinate value along the Y axis of the second segment

The wrist flag defines FLIP or NOFLIP configuration according: ---

FLIP – value of joint 5 is negative

NOFLIP – value of joint 5 is positive

Command configuration flags of PUMA robot and their influence on motion are given by:

Flag(shortname)	0 (auto)	1	2
ArmCmd (acmd)	Keep the current configuration or choose the closest joint-target.	Joint-target is LEFTY	Joint-target is RIGHTY
ElbowCmd (ecmd)	Keep the current configuration or choose the closest joint-target.	Joint-target is BELOW	Joint-target is ABOVE
WristCmd (wcmd)	Keep the current configuration or choose the closest joint-target.	Joint-target is NOFLIP	Joint-target is FLIP

Singular points

Singular points are joint points where robot configurations can not be determined, these are robot positions where a small difference in one of the joint angles introduce change in robot configuration.

Looking from the other side, from the WORLD-space coordinates. In the vicinities of these points, small changes in location coordinates can introduce big changes in joint coordinates. Therefore moving in cartesian-interpolated mode (MOVES, PASS-THROUGH, CIRCLE) through these points can result in strong increase of joint velocities.

Typical RX90 poses

• 

  Configuration of the robot at the "ready position" Joint: {0, -90, 0, 0, 0, 0} Cartesian: #{0 , 0 , 985 , 0 , 0 , 0}

• 

  Configuration of the robot at the "zero position"Joint: {0,0,0,0,0,0}:Cartesian: #{450, 0 , 535, 0, 0, 0}

• 

  LEFTY and RIGHTY configurations of same point: joint: {0,45,135,0,90,0} joint: {180,135,45,0,-90,-180}cartesian: #{233,0,-768,-180,90,180} cartesian: #{233,0,-768,180,90,-180}

• 

  joint: {0,-90, 90,0,90,0} joint: {0,-90, 90,90,90,0} cartesian: #{85, 0, 900,0, 90, 0} cartesian: #{0,85,900,90,90,0}

Geometric, Denavit-Hartenberg (DH) parameters of the PUMA robot

Usually in robotics DH parameters are used to describe the robot geometry. We are using the following definition of DH parameters:

• θ _i is the joint angel from the x_i-1 axis to the x_i axis about the z i-1 axis (using the right-hand rule).
• α _i is the offset angle from z i-1 axis to the z i axis about x_i axis (using the right hand rule).
• a_i is the offset distance from the intersection of the z i-1 axis with the x_i axis to the origin of the ith frame along x_i axis (or the shortest distance between the z i-1 and z I axes).
• d_i is the distance from the origin of the (i-1)th coordinate frame to the intersection of the z i-1 axis with the x_i axis along z i-1 axis.

For RX90 family of Staubli robots we have:

DH parameter	MC-BASIC property name	Value in mm
d2	<robot >.link[1][1]	0
a1	<robot >.link[1][2]	0
a2	< robot >.link[2][1]	450
a3	< robot >.link[3][1]	0
d4	< robot >.link[4][1]	450
d6	< robot >.link[6][3]	85

The above parameters of link matrix property of the robot must be filled-in in the robot’s setup file. In order that the system accepts the above setup the “configgroup” command must be executed. Note that only the above set of parameters affects the robot geometry, the rest of the link[][] and axis[][] values do not influence the robot geometry in any way and are not affected (changed) by configgroup command or any other robot command.

The rest of the DH parameters are predefined as follow and are automatically internally set by choosing the right group model.

Joint i	θi	αi
1	90	-90
2	0	0
3	90	90
4	0	-90
5	0	90
6	0	0

Orientation Angles

DELTA robot model

The DELTA robot is a four – degrees of freedom parallel robot of the following configuration:

The basic idea behind the Delta parallel robot design is the use of parallelograms. A parallelogram allows an output link to remain at a fixed orientation with respect to an input link. The use of three such parallelograms restrains completely the orientation of the mobile platform which remains only with three purely translational degrees of freedom. The input links of the three parallelograms are mounted on rotating levers via revolute joints. The revolute joints of the rotating levers are actuated in two different ways: with rotational motors or with linear actuators. Finally, a fourth leg is used to transmit rotary motion from the base to an end-effector mounted on the mobile platform.

WIPO patent issued on June 18, 1987 (WO 87/03528):

“The device comprises a base element (1) and a movable element (8). Three control arms (4) are rigidly mounted at their first extremity (15) on three shafts (2) which may be rotated. The three assemblies each formed by a shaft (2) and an arm (4) are the movable parts of three actuators (13) of which the fixed parts (3) are integral with the base element. The other extremity (16) of each control arm is made integral with the movable element through two linking bars (5a, 5b) hingedly mounted on the one hand to the second extremity (16) of the control arm and, on the other hand, to the movable element. The inclination and the orientation in the space of the movable element remain unchanged, whatever the motions of the three control arms may be. The movable element supports a working element (9) of which the rotation is controlled by a fixed motor (11) situated on the base element. A telescopic arm (14) connects the motor to the working element.”

Also: (US 4,976,582), and (EP 0 250 470).

ABB Flexible Automation's IRB 340 FlexPicker

DELTA Work-Space

Delta Workspace assessment

The Delta kinematics works with points of the XYZR robot type(X, Y , Z and Roll) where the first three coordinates are always expressed in millimeters and the roll angle in degrees.

The DELTA WORLD workspace is limited by joint limits and the following auxiliary cartesian boundaries, available for the user:

• RMAX – maximum radius of a cartesian point ([[Image:]]), no points beyond this radius are allowed.
• RMIN - smallest radius of a cartesian point ([[Image:]]). No points inside this radius are allowed.
• ZMIN – z coordinate of the “lowest” robot position. No points below this plane are allowed.

Robot configurations

Delta robot has no configuration flags, there is always one unique solution of the inverse kinematics problem. This further means that the configuration arguments in inverse kinematics function ToJoint have no influence on the result. Also Cartesian movements like MOVES, CIRCLE or PASS THROUGH are not influenced by any of the command configuration flags (acmd, ecmd, wcmd), the motion will be the same independent of the value of any of these flags.

Singular points

Delta robot, contrary to the open-kinematics chain robot has no singular joint positions where the joint velocities tend to infinity but there are so called “direct kinematics singularity” where the stiffness of the robot is significantly reduced.

Case when the upper arm linkages are parallel to each other. At this singular configuration manipulator can not resist any force applied in the plane of moving platform.

Another singular position which in is more a theoretical in our case is the so called direct kinematics singular configuration, [[Image:]]in which all the upper arm linkages are in the plane of the moving platform. At such a configuration, the manipulator actuators can not resist any force applied to the moving platform in the z-direction.

This case is possible only when which in our case is not fulfilled!

Geometric parameters of the DELTA robot

[[Image:]]

For DELTA family of robots we have:

parameter	MC-BASIC property name	Value in mm
Rbig	<robot >.link[1][1]	370
L1	< robot >.link[2][3]	300
L2	< robot >.link[3][3]	800
Rsmall	< robot >.link[4][1]	50

[[Image:]]

The above parameters of link matrix property of the robot must be filled-in in the robot’s setup file. In order that the system accepts the above setup the “configgroup” command must be executed. The model is assuming that the three base motors are rotated for exactly 120 degrees. Also it has been assumed that all three lags are of the same dimensions. The WORLD coordinate system is based in the center of the motor plane. Note that only the above set of parameters affects the robot geometry, the rest of the link[][] and axis[][] values do not influence the robot geometry in any way and are not affected (changed) by configgroup command or any other robot command.

Typical DELTA poses

[[Image:]]Farthest (highest) reachable Z coordinate and the maximum joint angle.

[[Image:]]

[[Image:]]

Closest (lowest) reachable Z coordinate and the maximum joint angle.

If [[Image:]] then the following is true (not applicable in our case):

[[Image:]]

[[Image:]]

[[Image:]]

“Zero” position, for [[Image:]]

[[Image:]]

[[Image:]]

Having a robot with following geometry:

Parameter	Value in mm
Rbig	370		Z0	505.56899
L1	300		Zmax	1052.4258
L2	800		Zmin	N/A
Rsmall	50		θmax	106.87532°

Test:

-->?ToCart({0,0,0,0})

#{-5.68434e-14 , 0 , 505.569 , 0}

-->?toCart({106.87,106.87,106.87,0})

#{-7.10543e-15 , 0 , 1052.43 , 0}

-->

Orientation Angle

The DELTA kinematics has only one orientation angle (roll) it is directly (one-to-one) driven by the fourth motor. The value of the WORLD roll angle equals always to the fourth joint position limited (wrapped) to angle values between [-180, 180]. Orientation interpolation is determined by the OrientationFollwing flag. In the case of DELTA robots joint and world angles are directly related and therefore we have only two options of the OrientationFollowing flag:

• Take the longer path between initial and final roll angle: OrientationFollowing flag equals 2 or 3
• Take the shorter path between initial and final roll angle: OrientationFollowing flag equals 0 or 1

Trajectory Interpolation

Joint interpolated (natural) motion MOVE

The MOVE instruction causes a joint-interpolated motion. That is, intermediate command points between the initial and final robot locations are computed by interpolating linearly between the initial and final joint positions. Any changes in configuration requested by the program are executed during the motion. If the MOVE is given on joint coordinates then the command configuration (acmd, ecmd, wcmd) flags must agree (be the same or “auto”) with the configuration flags of the given target point. Joint interpolated movement is determined by the regular MC group parameters (vcruise, acc, dec, jerk).

Linear interpolated motion MOVES

The MOVES instruction causes a straight-line motion. During such a motion, the tool center point is moved along a straight-line path and is smoothly rotated to its final orientation. Changes of configuration are not allowed during straight-line motions, except in cases when the commanded configuration flags are given with “auto” value and the movement allows it. Straight-line movement is determined by the pairs of translation/rotation parameters for velocity, acceleration and jerk: (vtran, atran, dtran, vrot, arot, jrot).

Circular (arc) interpolated motion MOVES

The CIRCLE instruction causes a circular or arc motion. During such a motion, the tool center point is moved along a arc (or circle) path and is smoothly rotated to its final orientation. Changes of configuration are not allowed during circular-line motions, except in cases when the commanded configuration flags are given with “auto” value and the movement allows it. Note that in cases of circular interpolation tool orientation is rotated around vector normal on the circle plane from its initial to its final value. Circular movement is determined by the pairs of translation/rotation parameters for velocity, acceleration and jerk: (vtran, atran, dtran, vrot, arot, jrot).

ORIENTATION INTERPOLATION DESCRIPTION

Having the initial and final orientation expressed as quaternions (Q_i, Q_f) the orientation interpolation will be done according to the formula:

[[Image:]]

Where vector r and angle phi are defined from:

[[Image:]]

The rate of change of angle phi is proportional to the rate of change of the WORLD position (X,Y,Z) of the robot movement.

The angle phi of the above formula can be always changed in two directions: positive & negative. One will be shorter the other longer, but this does not necessary give shorter movements in joint space. Therefore the user has the ability to influence orientation interpolation using the OrientationFollowing property of the robot. The values of the orientation-following property are:

0 – shortest path in world space (smallest phi).

1 – shortest path in joint space (choose such phi that final change of joint angles is minimal).

2 – longer path in world space (the other solution of phi)

3 – longer path in joint space (the other solution of phi)

GENERAL REMARKS ABOUT MOVEMENTS

There are four different combinations of interpolation type and target points. The target point can be given in world or in joint coordinates and the interpolation can be straight-line or joint-interpolated (natural). For both cases of straight-line (cartesian interpolation) motion the following rules apply:

• The actual value of Arm - flag (afbk) does not change during the motion. It’s initial and final value must be the same, except the case when the motion starts form an “auto” value of the flag.
• The actual value of Elbow - flag (efbk) does not change during the motion. It’s initial and final value must be the same, except the case when the motion starts form an “auto” value of the flag.
• The actual value of Wrist –flag (wfbk) does not change during the motion it’s initial and final value must be the same, except two cases: when the motion starts form an “auto” value of the flag. when the wcmd is given with an “auto” value and the motion passes through wrist singularity (pcmd{5} =0) not affecting the fourth and the sixth joint.

Straight line motion with joint target (MOVES {…,…,…})movement is interpolated in Cartesian space so the changes of XYZ coordinates are linear. User can specify configuration command flags (acmd, ecmd, wcmd) but they must comply with the given target point. If from some reason (usually it is the orientation-following ) the given target point is not reached in joint space (in world space it is always reached!) an error will be generated. Typically the last three joints are reached in other values which differ for ±360 degrees, but the Cartesian point is the same.

In this case the OrientationFollwing flag is ignored and all computation will be done as the user set the value of 1 (closest path in joint space).

Straight line motion with Cartesian target (MOVES #{…,…,…})movement is interpolated in Cartesian space so the changes of XYZ coordinates are linear. User can specify configuration command flags (acmd, ecmd, wcmd), only the proper setup will be allowed, else an error will be returned.

Joint –interpolated motion with joint target (MOVE {…,…,…})movement is interpolated in Cartesian space so the changes of j1,…,j6 are linear. The configuration flags are ignored.

Joint –interpolated motion with Cartesian target (MOVE #{…,…,…})movement is interpolated in Cartesian space so the changes of j1,…,j6 are linear. The configuration flags are taken into account when computing target coordinates. If “auto” configuration flag is selected the point closest to the target will be chosen.

Case of invalid selection of configuration flags.

Normally the system does not allow configuration flag change during cartesian-interpolated motions (MOVES, CIRCLE, PASSTHROUGH) if such a change is commanded an error will be returned and the motion will be not started. This occurs when the command configuration flags (acmd, ecmd, wcmd) differs from the feedback configuration flags at the start point (afbk,efbk,wfbk). However exceptions exist when the command flags are given with “auto(0)” value, depending on the specific robot model, the system chooses the same configuration as in the start position.

In PUMA kinematics, there are special cases when the configuration flags can change during the cartesian-interpolated motion.

A change of the WRIST flag can be obtained when the wcmd is set to “auto(0)” and issuing one of the straight line motions. If such a motion passes thorough zero of the fifth joint angle there is a possibility that the wrist flag is changed. However, this can sometimes introduce opposite rotations of joints 4 and 6. If this rotation is too fast an error message will be returned and the motion will be canceled. In order to prevent it user can command low Cartesian speed (vrot, vtran) near these points.

A change of the ARM and ELBOW flags can be obtained in the case of straight-line motion (MOVES) going through the arm point singularity which in case of PUMA robot with d2=0 is point X = 0, Y = 0, Z =a2+d4+d6 , or in case d2≠0 (RX60) is a circle X = d2 cos α, Y = d2 sin α, Z = a2+d4+d6.

[[Image:]]

Movements with the target point given in joint coordinates are treated differently. In these cases target joint coordinates and command configuration flags must comply (be the same or have auto value). If this is not the case motion will be not executed and an error will be returned.

User Frame system

The softMC system is enriched with a set of user-definable working frames. All working frames are available both modally and nodally. All user frames must have the same location type (XYZ, XYZR, XYZYPR …) as the parent robot.

Working with robots the following user-definable frames are provided:

• World frame.Default Cartesian coordinate system. This frame is usually located at the robot base.
• Base frame.The frame of the robot from the user-point of view. Normally used to correct rotation or inclination of robot mounting. (Usable for ceiling mounted robots).
• Tool frame.Frame attached to the tool mounted on the robot, closely related to the tool geometry. Useful for describing tasks made by the attached tool (grinding, debarring, screwing, …)
• Machine-Table Frame.Frame related to the machine robot is working with.
• Work-Piece Frame.Frame related to the work-piece mounted on the machine robot is working with.

TOOL

BASE[[Image:]] [[Image:]]

WORLD

All location properties of the robot are always expressed using the whole user-frame system, therefore always in the Work-Piece Frame defined by the whole user-frame chain. Using the previously defined compound transformation we have:

here=MachineTable^-1:WorkPiece^-1:Base: here_world:Tool

[[Image:]]

Setting new values the tool property can cause a significant increase of position error (pe). The value of pe is always calculated at the tool-tip point, so increased tool length and small orientation errors can lead to an increased pe. This is obvious when putting pe into above equation: pe=MachineTable-1:WorkPiece-1:Base: peworld:Tool

TOOL

Tool is a Robot property which informs the system to use the specified location as the tool transformation. It defines the position and orientation of the tool tip in relation to the center of the tool flange. The default tool transformation is the NULL transformation, which can be expressed as TOOL = #{0,0,0,0,0,0}

Example:

Puma.tool = #{25,0,100,0,45,0}

BASE

Base is a Robot property which informs the system to use the specified location as the base transformation. It defines the position and orientation of the arm in the cell according to the WORLD reference.

The default base transformation is the NULL transformation, which can be expressed as Puma.base = #{0,0,0,0,0,0}

MachineTable

MachineTable is a Robot property which informs the system to use the specified location as the machinetable transformation. It defines the position and orientation machine working frame relative to the BASE reference.

The default machinetable transformation is the NULL transformation, which can be expressed as Puma.MachineTable = #{0,0,0,0,0,0}

WorkPiece

WorkPiece is a Robot property which informs the system to use the specified location as the workpiece transformation. It defines the position and orientation of work-piece relative to the MACHINE TABLE reference.

The default base transformation is the NULL transformation, which can be expressed as Puma.WorkPiece = #{0,0,0,0,0,0}

So we have:

P_base=Base:P_world:Tool

P_base=MachineTable:WorkPiece:P_WorkP

For the given setup user frames will be:

Base = #{300,200,1000,0,180,180}

Tool = #{50,0,0,90,45,0}

MachineTable = #{400,100,0,0,0,0}

WorkPiece = #{20,70,150,0,90,-90}

Therefore we have:

P_WP= WorkPiece^-1: MachineTable^-1:Base:P_World:Tool

Side effects

Setting the any of the user frames modally (tool, base, machinetable, workpiece), have an immediate effect on all the position computations:

• Setpoint
• Here
• Start
• Dest
• ToJoint

but doesn’t affect the current motion.