Element Coordination/Absolute Movements During Conveyor Tracking

Absolute movements during Conveyor Tracking

 

Let's define the  term "total robot position" to denote Cartesian position of the robot which when translated into joint values represents the position coordinates that are actually sent to drives over SERCOS. So the total position during Conveyor Tracking is composed from two things: robot motion and conveyor motion. Where robot motion is the coordinate change introduced by user issuing different motion commands (MOVE/MOVES/...) and conveyor motion is the motion of conveyor in different phases (syncing, tracking, de-syncing). Written in formulas:

 

Total = Robot + Conveyor  

The Conveyor coordinates are:

Conveyor(t) = CNV(t) - CNV(t0) where t0 is the moment of issuing "slave=5"

 

And the CNV is:

CNV(t) = (DownStream - UpStream)*(Master(t) - Trigger)/(DownMaster-UpMaster)

Master(t) - position of conveyor master axis at moment t.

DownStream, Upstream, DownMaster, UpMaster - parameters of conveyor.

 

The relation to MC-BASIC language variables is obvious:

 

<mf>.here = CNV(t)

<mf>.zero = CNV(0) (See MovingFrame.ZERO)

 

What is general idea behind that? Simple, the total robot motion of robot that began to track (slave=5, synced or not) is the SAME as position of free robot (slave=0) IF the conveyor did not move. In other words, if you want to teach some points on conveyor just stop the conveyor and move the robot to it's destination on the belt.

The robot motion, i.e. the "Robot" variable in the main formula is the robot absolute position during conveyor tracking, and it equals to the position entered as a target point of the motion commands (MOVES/CIRCLE/...)

Case of One Conveyor

So having General formula Total = Robot + Conveyor, it is interesting to see what happens in different motion types. First the Total is a Cartesian point of the robot it's correspondent joint point is:

Total = InverseKinemaitcs(Total,cfg)

where cfg is the current commanded set of configuration flags depending on robot model (arm, elbow, wrist, ...).

General description of motion commands

Every motion command (MOVE, MOVES, CIRCLE, DELAY, ...) consist of two pair of parameters: Cartesian & joint target and initial position. Target position is the position user enters ans an argument of motion command and the initial point is the position from where the given motion starts. We denote these variables as:

Target = DirectKinemaitcs(target)

Start = DirectKinemaitcs(start)

It is clear that the target position of previous motion command (previous motion package) is the initial(start) position of the current motion, i.e.:

startⁱ = target^i-1

Startⁱ = DirectKinemaitcs(startⁱ) = DirectKinemaitcs(target^i-1)

The above formulas are true for any kind of motion, both in cases when robot is tracking conveyor or not. There is only slight differences if the target point is given as Cartesian or as joint point. Means if we are using direct or inverse kinematics transformation.

Because of different tool-frame settings (tool, base, workpiece, machinetable, ...) we always get the Cartesian initial position of the movement (Start) from the joint target coordinates of previous motion:

Startⁱ = DirectKinemaitcs(target^i-1)

Motion commands during conveyor tracking

The above formulas describing general motion command define actually how the Robot point in (main conveyor tracking formula ) is computed. For different motion type it differs:

Motion Type	Interpolation Formula
MOVES	Robot = Start + (Target-Start)*l
MOVE	Robot = InverseKinemaitcs(start + (target-start)*l,cfg)
DELAY	Robot = Target = Start
CIRCLE	....

where l is the profiler path variable, running always from 0 to the path's total length L.

In case of free robot (means robot that is not tracking any conveyor) we have: Total = Robot and therefore:

Total = InverseKineamitcs(Robot,cfg)

this implicitly means that Cartesian point Robot must be within robot's working envelope, i.e., there must exist at least one solution of Inverse Kinematics Problem for that point.

When robot is tracking a conveyor, this does not have to be true, in this case we do the inverse kinematics transformation of the sum: Robot+Conveyor, which implies that the Cartesian point "Robot" does not have to be reachable! This further means that formulas of (internal motion package relations) are not true anymore. In this case we have two new motion package variables, called Offset_start and Offset_base which compensate (offset)  conveyor motion:

startⁱ = target^i-1

Startⁱ = DirectKinemaitcs(startⁱ) + Offset_baseⁱ - Offset_startⁱ

and the target point of the movement is, if given by the user as Cartesian point:

Target = TargetU + Offet_base

target = InverseKineamitcs(Target)

where Target^U is the Cartesian target point given by the user.

In case when target point is given by the user in joint coordinates we have:

Target =DirectKineamatics(target)

Target^U=DirectKineamatics(target) - Offet_base

As with target point the start point has also user presentation and the internal value. Although here the term "user presentation" is not accurate as the value is not given by the user but just defined as:

Start^U = Start - Offset_base

or:

Start^U = DirectKinemaitcs(start)- Offset_start

To summarize all this would be that user-given target point is offset-ed in order to get kinematically reachable Cartesian point (one that has solvable inverse kinematics). The Offset_base used is obtained as the current offset introduced by conveyor motion at the moment of issuing of the motion command, while the other offset variable, the Offset_start is just previous value of the Offset_base:

For MOVES & CIRCLE:

Offset_baseⁱ = Conveyor(t)

where t is the moment when the movement is issued

Offset_startⁱ = Offset_base^i-1

For MOVE & DWELL:

Offset_baseⁱ = Offset_base^i-1

Offset_startⁱ = Offset_base^i-1

here we just forward the Offset_base of the previous motion to then next!

Interpolation of different motion types now looks like:

Motion Type	Interpolation Formula
MOVES	Robot = Start + (Target-Start)*l  - Offset_base
MOVE	Robot = InverseKinemaitcs(start + (target-start)*l,cfg)  - Offset_start
DELAY	Robot = Target = Start
CIRCLE	....

 

Let's Check the above settings using zero-motions (motions where target equals start point):

For MOVES:

Robot = Start - Offset_base = StartU = DirectKinemaitcs(start)- Offset_start 

if this is the first motion after engaging conveyor the Offset_start is zero and therefore:

Robot = DirectKinemaitcs(start)

The target point is also simple

Robot = Target - Offset_base = TargetU

means,  the "Robot" Cartesian point gets the target value user entered. 

A second MOVES motion smoothly continues:

Robot = Start - Offset_base = Start^U = DirectKinemaitcs(start) - Offset_start  = DirectKinemaitcs(targetprev) - Offset_baseprev 

because:

start  = targetprev 

Offset_start  = Offset_baseprev 

as defined.

 

Switching into joint-interpolated move is little bit trickier:

Robot =  DirectKinemaitcs(start) - Offset_start  = DirectKinemaitcs(targetprev) - Offset_baseprev 

that is same is in MOVES, but target is:

Robot =  DirectKinemaitcs(target)  - Offset_start 

according  the interpolation formulas during tracking (using Offset_start instead Offset_base). Therefore:

Robot = TargetU =DirectKineamatics(target)   - Offet_base

Desyncing of a conveyor (Virtual stop)

      When a conveyor is disengaged from robot it performs a virtual stopping profile, according to <robot>.AccelerationDesync and <robot>.JerkDesync parameters. The initial velocity is taken to be the conveyor velocity at the moment of disengagement start. The conveyor performs a profiled commanded motion towards zero velocity, while being disconnected from the real master source from the moment of disengagement start.

 

  

Case of Two Conveyors

 

What happens during switching to another conveyor? Then, we have one conveyor de-syncing and the other syncing at same time:

Total = Robot + Conveyor₁ + Conveyor₂

Where:

Conveyor₁(t) = CNV1(t) - CNV1(t₀₁)

And

Conveyor₂(t) = CNV2(t) - CNV2(t₀₂)

Where t₀₂ is the moment of issuing "masterframe = cnv2" and t₀₁ is the moment of engaging the first conveyor either by issuing "slave=5" or by setting "masterframe = cnv1" while <robot>.slave was at "5".

      This situation can be detected when <robot>.IMFS flag equals -2. The IMFS flag is zero when the robot is not engaged with any conveyor or when it is syncing to one, it equals to 1 when the robot is synced to a conveyor and equals to -1 when it is desyncing from a conveyor without being engaged with any other conveyor.

 

It is clear that at some moment of time "ts1" the first conveyor will complete it's virtual deceleration and we will switch to one-conveyor situation back:

Total = Robot + Conveyor₂

To assure continuity of robot motion we must assure no jumps, i.e.:

 

Total*(ts1) = Total(ts1)=> Robot*(ts1) + Conveyer₂(ts₁) = Robot(ts₁) + Conveyor1(ts₁) + Conveyor₂(ts₁)

From there:

Robot*(ts₁) = Robot(ts₁) + Conveyor1(ts₁)

      This means that at the moment of switching between two conveyors (more exactly at the moment when robot tuns to be engaged only with the second one) we have a correction of robot's current packages positions according to:

 

start = start - Offset_base + Conveyor₁(ts1)      = startU   + Conveyor₁(ts₁)

target = target - Offset_base + Conveyor₁(ts₁)  = target^U + Conveyor1(ts₁)

Offset_base = 0

Offset_start = 0

where  Conveyor₁(ts₁)  = CNV1(ts₁) - CNV1(t₀₁)  (actually first conveyor virtual stopping position)

Same transformation happens when the robot is disengaged from conveyor without engaging to another.

 Conclusion

During conveyor tracking total robot Cartesian position consists of two parts, the first part is the robot absolute position and the second part is the current conveyor relative position from it's zero (<cnv>.zero). The two parts are superimposed. When a conveyor disengages from a robot, its relative stopping position is added to the absolute robot position and becomes new total robot position.