Element Coordination

Introduction

The main goal of this feature is to synchronize both in space and in time movement of several robots together.This will be done using the standard master-slave relationship existing int he system and expanding it into a general robot-to-robot connection. Using the Moving Frame feature typically used in conveyor tracking by enabling the source of the moving frame to be any robot type.

The master robot is the master source of a moving frame that the slave robot follows. The moving frame mechanism will be used as a skeleton by dropping all synchronization stuff typically used in conveyor tracking. The slave robot is immediately synchronized to the moving frame by imposing additional condition that master-slave relationship can be only assigned when both robots are standing still (not moving).

The robot's mutual space-relationship will be defined through newly-added base and object location offsets of the moving frame. Together with it, mutual coordination of robots of different point types (world spaces) and/or different NDOF's will be possible.

Affected(changed) commands

Data Creation

There are two groups of Moving Frame objects in the aico.control system: axis-based and robot-based. Moving Frame (MF) is created in the same way as axes-based means using the command:

COMMON SHARED <mf name> as moving frame of <mf-type>

The difference between axes-based and robot-based moving frames in the argument supplied to the MasterSource command. In axis-based MF's only axes can be set as the master sources.Also, in the axis-based MF's the point-type of all point MF's properties (zero, here, Upstream, DownStream) are based on the point type of the MF (i.e. <mf-type>).

In robot-based MF's master-source is a robot element that has it's own point-type. In robot-based MF's the NDOF is ignored and the relation between MF's point-type and the robot's point type will be defined using the newly introduced properties (See: Translation Transformation , Orientation Transformation ).

However the biggest difference between axes-based MF's and robot-based MF's is that the later ones do not support on-the-fly synchronization to the moving frame. The engagement of a robot to a robot-based MF is possible in non-moving state only!

Master Source

ability to add <robot> elements and command (setpoint) and feedback(here) positions the following sytnax will be allowed:

<Moving Frame>.MasterSource = <robot>.setpoint and <Moving Frame>.MasterSource = <robot>.here

Setting the MasterSource to the one of the robot Cartesian-coordinates above will autlaticaly switch the Moving-Frame type to 3 and the <Moving Frame>.type will become read-only.

Note that point-type of the <robot> does not have to mach the point-type of the <moving frame>!

Slave

Setting <robot>.slave = 5 immediately enters into synchronization-tracking mode, no transitions phases. Syntax remain the same.

Trigger

Trigger command will be not allowed in case of robot-based moving frames.

Is Moving Frame Synchronized

Is Moving Frame Synchronized flag will return always 1 (true)

Is In Window

Is In Window flag will return always 1 (true), actually there is no concept of working windows in this type of <moving frame>

Unused Moving-Frame properties

• Upstream, Downstream, Upmaster, Downmaster
  

As the working-window concept does not exist in robot-based MF's these properties will have absolutely no influence on MF-robot operation.

• VelocityMaxTrans, VelocityMaxRot, AccelerationMaxTrans, AccelerationMaxRot, JerkMaxTrans, JerkMaxRot
  

Have no meaning on MF-robot operation.

• Dampingfactor, FilterFactor, MaxFlops
  

are sync-influencing parameters, as in MF-robot has no synchronization process.

NOI

In case of MF-robot NOI will return always -1.

New commands

Moving Frame Base Offset

• Moving-Frame BASE is an offset of the master robot to the slave

slave.robot = MS2MF( MF.base^-1 : MF.MSource:MF.ObjectLoc )

where function: A = MS2MF(B) is defined as:

A->position = p-projection * B->position
A->orientation= q-projection * MF.SLAVEORINETATION * B->orientation

• The * MF.SLAVEORINETATION represents multiplication by rotation matrix (quaternion, orientation-vector) for additional orientation vector manipulation.

• p-projection is a simple projection mapping that copies (X,Y,Z) into/from (X,Y) coordinatewise (X to X, Y to Y, Z to Z).
• q-projection is orientation vector projection, using quaternions it is easily represented by:

if: $Q_{XYZ}={\begin{bmatrix}\varphi &,&{\vec {n}}_{xyz}\\\end{bmatrix}}$ and: ${\vec {n}}_{xyz}={\begin{bmatrix}nx&ny&nz\\\end{bmatrix}}$

Then: $Q_{XYZ}:={qprojection}(Q_{XY})={\begin{bmatrix}\varphi &,&{\vec {n}}_{xy}\\\end{bmatrix}}$ where ${\vec {n}}_{xy}$ by projecting the unit vector ${\vec {n}}_{xyz}$ tot he XY plane and normalizing it again.

In other words the unit vector of orientation will be immersed into other's robot work-space (keeping it normalized, i.e. ||n|| = 1) while keeping the same rotation angle $\varphi$.

Due to the fact that projected unit orientation vector is in the same half-space the actual rotation angel sign will be updated.

For example:

Having Yaw-Pitch-Roll space with angles: #{0, 180,90} (vector turned down with roll of 90 degrees) will be projected into Roll space as:

#{-90} = q-projection (#{0, 180,90}) , this due to the fact that n_xyz is projected into [0,0,-1]

Object Description

Object moved by two robots or the relative position of two robot's end-points to each other is described by OBJECTLOC Moving-Frame property. It describes the relative position and location of the slave's setpoint from the actual master's setpoint coordinates.

See Also: Object Location Property

Point Type transformations

Contrary to the ordinary axes-MF robot-MF can have different point type from the slaved robot. In order to transform from MF point type into slave-robot point type new property has been defined. The transformation is looking for the closest location inside slave-robot working space.

Slave Orientation

MF.SLAVEORIENTATION is a fixed type YPR (Yaw-Pitch-Roll, See: Yaw Pitch Roll) orientation vector used to rotate moving frame orientation vector into slave-robot orientation coordinates.

Let's define the product of moving frame master source as intermediate SR variable:

SR := ( MF.base^-1 : MF.MSource:MF.ObjectLoc )

then the SR variable will be of the same point-type as the moving frame source MF. in order to translate it into slave's robot command we do:

slaverobot.position = p-projection(SR.position)
slaverobot.oreintation = q-projection(MF.SLAVEORINTATION*SR.orientation)

	NOTE
	In case when number of position coordinates of MF is less then of MasterSource the remaining coordinate will be assigned by the MF.BASE corresponding coordinate value.

	NOTE
	In case when number of position coordinates of MF is less then of MasterSource the remaining coordinate will be assigned by the MF.base corresponding coordinate value.

Side effects

• Zero

Coordinate Transformations

Slave robot is connected to a master through Moving-Frame assigned to the slave's master frame property:

<slaverobot>.MasterFrame = <master frame>

Master frame is a link between slave and master robot. It Includes definition of the master-source which can be either robot Cartesian command point (SETPOINT) or robot's Cartesian feedback (HERE):

<master frame>.MasterSource = <masterrobot>.SETPOINT

BASE transformation

Master frame defines spacial relationship (distance and rotation) of the slave's robot frame to the master: <Master Frame>.Base

File:AXY;SOWEMA COOP 2 ROB BASE.png

Two robots with a defined distance between them using MF.BASE

Defining an Object

In total the object is held by two robots from two sides. Looking from the master's coordinate frame we have twp points:

<masterrobot>.setpoint
<masterrobot>.setpoint:<movingframe>.objectloc

The second point of the object can be also reached through salve robot by:

<movingframe>.base:<slaverobot>.setpoint

File:AXY;SOWEMA COOP 2 ROB OBJ.png

Usage of OBJECTLOC

<Moving Frame>.ObjectLoc is a property of the Moving Frame data-object of the type location of the same point-type as the moving frame. It means if the moving frame was declared as XYZR, for example, the OBJECTLOC will be also of type XYZR.

The OBJECTLOC specifies position and orientation of the slave's end-effector relative to the master's end point (Master Frame actually).

Example of complete slave command calculation

File:AXY;SOWEMA COOP 2 ROB FRM.png

Frame calculations in two-robot case.

mf.base = #{100,100,0,45}
mf.base^-1 = #{-141,100,0,-45}
mf.objectloc = #{80,0,0,180}
master.setpoint = #{130,-72,0,0}

slave.setpoint = mf.base^-1:master.setpoint:mf.objectloc
slave.setpoint = #{-43 , -200 , 0 ,135 }

Example of three robots working on a same object

Here is an example of three-robot cooperation. One robot is defined as a master and the other two are slaves. Each of the slave robots is connected through it's master frame to the master robot on different distances from it and on different gripper-points on the object.

File:AXY;SOWEMA COOP 3 ROB.png

Frame Calculation in three-robot case

In this example we will define two Moving frame with each of them having a different OBJECTLOC.

The first Moving Frame:

Common Shared MF1 As Moving Frame Of XYZR
MF1.MasterSource = <masterrobot>.SETPOINT
MF1.BASE = #{..., ..., ..., 0}
MF1.OBJECTLOC = #{L,..., ..., 180}

The second Moving Frame:

Common Shared MF2 As Moving Frame Of XYZR
MF2.MasterSource = <masterrobot>.SETPOINT
MF2.BASE = #{..., ..., ..., 0}
MF2.OBJECTLOC = #{SQRT(3)*L,L/2, ..., -150}

• Assuming the object is even-sided (Equilateral) triangle of the side length L.

	NOTE
	The orientational part of the second object-location MF's it is -150 degrees. The first slave's robot Xtool axis is in exactly opposite direction to the master's Xtool axis. And the second slave robot's Ytool axis is in exactly opposite direction to the master's Ytool axis.

Examples

Example 1.

Cooperation between two unequal robot kinematics.

Moving Frame definition:

Common Shared MF as Moving Frame of XYZR

Linking it to the source (master):

MF.MasterSource = Puma.Setpoint

Now the master-source is a puma robot of point-type XYZYPR and the linked robot or the MF is of XYZR point type. So we need to define transoformation matrices for both position and orientation:

Transformation matrix for position is:

${\begin{bmatrix}X\\Y\\Z\\\end{bmatrix}}={\begin{bmatrix}1&0&0\\0&1&0\\0&0&1\\\end{bmatrix}}\cdot {\begin{bmatrix}X\\Y\\Z\\\end{bmatrix}}$

Transformation matrix for orientation is:

$Roll={\begin{bmatrix}0&0&1\\\end{bmatrix}}\cdot {\begin{bmatrix}Yaw\\Pitch\\Roll\\\end{bmatrix}}$

Or in the code:

for orientation

' First row:
MF.OrientationTransformation[1][1]=0
MF.OrientationTransformation[1][2]=0
MF.OrientationTransformation[1][3]=1

for translation:

' First column:
MF.TranslationTransformation[1][1]=0
MF.TranslationTransformation[2][1]=0
MF.TranslationTransformation[3][1]=0
' Second column:
MF.TranslationTransformation[1][2]=0
MF.TranslationTransformation[2][2]=0
MF.TranslationTransformation[3][2]=0
' Third column:
MF.TranslationTransformation[1][3]=0
MF.TranslationTransformation[2][3]=0
MF.TranslationTransformation[3][3]=1

And then the offsets definitions:

The XY coordinates will be offseted for (10,10) mm:

MF.base = #{10,10,0,0}

A distance of 100mm between the two robots tool-tips is defined as:

MF.ObjectLoc = #{0,0,100,0}

and the tracking (master-slave) is defined by:

Scara.Masterframe = MF

and started:

Scara.slave = 5

Example 2.

Moving Frame definition :

Common Shared MF as Moving Frame of XYZR

Having a tilt table robot (Object Having only Yaw and Pitch angle):

Common Shared TiltTable As Group Axnm = Ax5 Axnm = Ax6 Model = 1 of YP

Linking it to the source (master):

MF.MasterSource = TilTable.Setpoint

Now the master-source is a puma robot of point-type YP and the linked robot or the MF is of XYZR point type. So we need to define transoformation matrices for both position and orientation:

Transformation matrix for position actually does not exist as the TiltTable have no position component!

Transformation matrix for orientation is simple, we actually do not want to have any influence of table's Yaw and Pitch angles, therefore:

$Roll={\begin{bmatrix}0&0\\\end{bmatrix}}\cdot {\begin{bmatrix}Yaw\\Pitch\\\end{bmatrix}}$

Or in the code:

for orientation

' First row:
MF.OrientationTransformation[1][1]=0
MF.OrientationTransformation[1][2]=0

And then the offsets definitions:

The XY coordinates will be offseted for 200 mm:

MF.base = #{-200,0,0,0}

Defining the object-location of 100mm makes the trick!

mf.base = #{Xb,Yb,Zb,Rb}
mf.objectloc = #{Xo,Yo,Zo,0}
master.setpoint = #{yaw,pitch}

slave.setpoint = mf.base^-1:master.setpoint:mf.objectloc

i.e. the vector [Xo,Yo,Zo] will be rotated by the tilt-table angles [yaw,pitch]!!!

and the tracking (master-slave) is defined by:

Scara.Masterframe = MF

and started:

Scara.slave = 5