You are on page 1of 72

Motion Control Library

Tutorial

January 2007 (Ver. 1.0)

Notice
This tutorial is delivered subject to the following conditions and restrictions: This tutorial contains proprietary information belonging to Elmo Motion Control Ltd. The text and graphics included in this manual are for the purpose of illustration and reference only. The specifications on which they are based are subject to change without notice. Elmo Motion Control and the Elmo Motion Control logo are trademarks of Elmo Motion Control Ltd. Information in this document is subject to change without notice. Document No. MAN-MLT Copyright 2007 Elmo Motion Control Ltd. All rights reserved.

Revision History:

Ver. 2.0 Ver. 1.0

Jan. 2007 June 2004

(MAN-MLT.pdf) (MAN-MLT.pdf)

Updates Initial Release

Elmo Motion Control Inc. 1 Park Drive, Suite 12 Westford, MA 01886 USA Tel: +1 (978) 399-0034 Fax: +1 (978) 399-0035

Elmo Motion Control GmbH Steinkirchring 1 D-78056, Villingen-Schwenningen Germany Tel: +49 (07720) 8577-60 Fax: +49 (07720) 8577-70

www.elmomc.com

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Contents
Chapter 1: General Description .............................................................................................. 1 1.1 Introduction ............................................................................................................ 1 1.2 Vector properties ...................................................................................................... 1 1.3 Trajectory generation ............................................................................................... 3 1.3.1 Line................................................................................................................ 3 1.3.2 Circle ............................................................................................................. 3 1.3.3 Spline............................................................................................................. 3 1.3.3.1 Examples for the two-dimensional spline interpolation ............. 5 1.3.3.2 Examples of three-dimensional spline interpolation ................... 6

1.3.4 Polyline ............................................................................................. 10 1.3.4.1 Examples for the two-dimensional polyline ............................... 12 1.3.4.2 Examples for the three-dimensional polyline............................. 14
1.4 Transition to a new trajectory with a non-zero velocity..................................... 16 Chapter 2: Switch Radius Calculation ................................................................................ 2-1 2.1 Line line intersection .......................................................................................... 2-1 2.2 Circle line intersection ....................................................................................... 2-6

2.2.1 Line goes inside the circle ............................................................... 2-6


2.2.1.1 Switch arc center and circle center belong to two different half planes defined by the line L ................................................................... 2-6 2.2.1.2Switch arc center and a circle center belong to the same half plane............................................................................................ 2-12 2.2.1.3 Line intersects the center of the circle...................................... 2-17

2.2.2 Switch arc radius calculation by the distance from the intersection point .220
2.2.2.1 Initial circle center and switch arc center belong to the same half-plane ............................................................................................... 2-20 2.2.2.2 Initial circle center and switch arc center belong to two half planes defined by the line L. ................................................................ 2-22 2.2.2.3 Circle center (Xc,Yc) L1 (line L1 intersects the center of the circle)................................................................................................... 2-24 2.2.3 Line goes outside the circle .................................................................. 2-24 2.2.3.1 Line L and init radius continued in their positive intersecting directions................................................................................................ 2-25 2.2.3.2 Line parallel to the circle arc init radius ....................................... 2-27 2.2.3.3 Line L and init radius continued in their reverse directions intersect 2-28 2.3 Circle circle intersection.................................................................................. 2-30 2.3.1 One of two circle arcs intersects the internal area of the second...... 2-31 2.3.2 Each circle intersects the internal area of the second ......................... 2-39 2.3.3 No circle intersects the internal area of the other .............................. 2-42 Appendix A: Projection of a point on a line defined by the end points ........................ A-1 Appendix B: Coefficients of the line standard equation for the line defined by the end points ........................................................................................................................................B-1 Appendix C: Intersection point of two lines defined by the end points ....................... C-1 Appendix D: Circle line intersection points ................................................................... D-1

Maestro Motion Library Tutorial


MAN-INTUG (Ver. 1.7)

Chapter 1: General Description


1.1 Introduction
The Motion Library (ML) produces trajectories based on the PVT mechanism. It implements a set of functions that calculate trajectories as PVT tables for the vector motion. As a result of a call to the Motion Library functions, the user gets a PVT table for the requested trajectory. It supports two and three-dimensional vector motion. A PVT table is a two or three-dimensional sequence of PVT points. Each PVT point is defined by:

position value velocity for this position


time interval that is necessary to arrive from the current position to the position defined by the next PVT point

It supports 1. Single shape trajectories: line (2D,3D) circle (2D) 2. Trajectory built from an arbitrary set of pointes interpolated by the cubic spline (2D,3D) 3. Polyline trajectory that can include a number of single shapes: line segments (2D,3D) circle arc segments (2D) spline points (2D,3D) Inside polyline transition from one shape to another can be executed with a non-zero velocity. In this case, an additional element switch circle arc-- is inserted between two shapes. In the case of two-dimensional vector motion, switch arcs can be built for the line-line, line-circle, circle-line and circle-circle intersections. In the case of a three-dimensional polyline, switch arcs can be built for the line-line intersection.

1.2 Vector properties


Geometry of trajectory is defined by the set of vector functions such as circle() or line(). The Velocity profile is also influenced by the set of the following parameters (vector properties): maximum vector acceleration/deceleration (vac/vdc) maximum vector velocity (vsp) end velocity (vse)

Maestro Motion Library Tutorial


MAN-INTUG (Ver. 1.7)

general trajectory time (vtt) switch arc definitions (vsc, vsr, vsd) admissible velocity and position errors definitions (vpe,vve) PVT step low and high limits (VNT,VXT ) All of the vectors properties can be set in a user program or by the Command Interpreter. Syntax of a property: Vector_name.property Examples: v1.vsp - defines maximum vector velocity v1.vtt - defines trajectory time v1.vsc - defines smooth type from one shape to another (1 minimal radius switch arc, 2 fixed radius switch arc, 3 switch with a fixed distance from the intersection point). Single shape trajectories can be executed in one of three modes pre-defined by the value of the input parameter vum: 1 max velocity, 2 fixed time, 3 fixed velocity. In the maximum velocity mode, velocity defined by the parameter vsp is considered a limiting value that cannot be exceeded. If a trajectory is not long enough to achieve such a value, then a trajectory with a triangle velocity profile is built and some maximum vector velocity Vmax is achieved at one point.

< vsp

The fixed velocity mode (vum=3) is used if the user is interested in building a trajectory with a trapezium velocity profile the main part of the trajectory (with the exception of possible acceleration/deceleration at the initial and final parts of the trajectory) is executed with a velocity equal to vsp. If a trajectory is not long enough to reach velocity vsp with the given vector acceleration /deceleration (input parameters vac/vdc), the trajectory is not built and the user receives an error message. In the fixed time mode (vum = 2) the user must define parameter vtt time in milliseconds for the trajectory execution. The Motion Library chooses a velocity profile that satisfies parameter vtt. If a trajectory with the given length, maximum velocity vsp and vector acceleration/deceleration cannot be executed within time vtt trajectory is not built and the user receives an error message. The user can set values for the maximum PVT step in milliseconds parameter vxt and for the minimal PVT step parameter vnt 1msec. In this case, the main part of the trajectory will be executed with the PVT step T = 0.5(vxt

+ vnt).

For the switch arc, connecting two shapes can be chosen from one of the three possible modes predefined by the input parameter vsc: 0 no switch arc to be built, 1 switch arc with the minimal possible radius, 2 switch arc with radius pre-defined by the user, 3 user defined distance from the intersection point (for the line-line or circle-line intersections).

Maestro Motion Library Tutorial


MAN-INTUG (Ver. 1.7)

1.3 Trajectory generation


1.3.1 Line
Target position for a line is defined by the parameters of the function line(): Two-dimensional line V1.line(x,y) produces a line trajectory from the current position to the point (x,y), where x and y integer values in counts. Three-dimensional line V1.line(x,y,z) produces line trajectory from the current position to the point (x,y,z), where x,y and z - integer values in counts.

1.3.2 Circle
Radius, initial and sweep angles for a circle must be defined as parameters of a function circle(): V1.circle(radius, init_angle,sweep_angle), where init_angle and sweep_angle must be set in degrees (float), radius in counts (integer). Example (Motion Mathematic Lib Samples\ Vector_2D \CircleArc www.elmomc.com) v1.vac = 28000000 v1.vdc = 28000000 v1.vum = 1 v1.vsp = 250000 v1.vse = 0 //max acceleration //max deceleration //build trajectory in max. velocity mode //maximum velocity //end velocity

v1.circle(100000,45,-270) //build circle arc trajectory v1.bg // start motion

while (a1.ms==2)||(a2.ms==2) //wait until both axes have stopped wait(10) end while

1.3.3 Spline
A spline gives the possibility to move a smooth curve through an arbitrary set of points that do not necessary belong to a particular geometric shape as a circle, ellipse or a line. The spline that is supported by the Motion Library is an interpolation cubic spline. All the points

Po,P1,...,Pn given by the user belong to the spline curve. Between each pair of the neighboring points (Pi,Pi+1), cubic spline is defined by a third-order polynomial.

Maestro Motion Library Tutorial


MAN-INTUG (Ver. 1.7)

Other popular types of splines like Bezier curves, B- splines or NURBS are usually not interpolation but smoothing splines. The spline curve does not move through the given points but near them. So cubic splines are piecewise polynomial (built of cubic polynomials for each segment [i,i+1]) and produce a curve with continuous first and second derivatives at the internal control points P1, cubic spline polynomial for the segment [i,i+1] as Si(t) than Si(ti) and Si(ti)

P2 ,

, Pn-1 that, in case of motion control, means continuity of the velocity and acceleration. Denote a

= Si+1(ti), Si(ti) = Si+1(ti)

= S i+1(ti), i = 1,2,,n-1.

The spline trajectory is executed in the maximum velocity mode vum = 1. Input parameters that define kinematics of the trajectory are maximum velocity parameter vsp, vector acceleration parameter vac and vector deceleration parameter vdc. The Motion Library user can define points for the spline interpolation applying the following function calls: vector_name.splines(trj_name) - starts a sequence of points to be interpolated. The PVT table is saved in a file named trj_name. The function parameter trj_name can be missed. In this case, the trajectory is saved in a temporary file named vector_name.trj (where vector_name name defined in a resource file). To interpolate two-dimensional points, use a function call vector_name.splinep(int PosX, int PosY) ) adds interpolation point with coordinates (PosX, PosY). In three-dimensional space vector_name.splinep(int PosX, int PosY, int PosZ) ) adds 3D interpolation point with coordinates (PosX, PosY, PosZ). vector_name.splinee(parameter) - ends the spline trajectory sequence.
If parameter = 0 the standard PVT table that in each line contains PVT points for the X and Y axes (for 2D spline) or PVT points for X, Y and Z axes (for 3D spline) is built.

If parameter 0 three tables for the axis X, axis Y and for the gear are built. The table for the gear contains PVT points with the position equal to the distance along the spline from the initial spline position, vector velocity V = [Vx2 + Vy2]1/2 for this position and a standard time step (in 3D case four tables for X,Y, Z and gear are built). Inside the spline operator parenthesis splines(trj_name) and vector_name.splinee(parameter) can be added operators for the position calculation (for instance ellipse points X = Xc + acos(t), Y = Yc + bsin(t)).

The Motion Library generates a trajectory by the cubic spline interpolation Important note: Current position is not automatically added to the sequence of spline points. Homing must be done to the first spline point.

Maestro Motion Library Tutorial


MAN-INTUG (Ver. 1.7)

1.3.3.1 Examples for the two-dimensional spline interpolation


Example Example (Motion Mathematic Lib Samples\ Vector_2D \ Spline_Ellipse www.elmomc.com) Ellipse trajectory (2D spline interpolation)

v1.vum=1 //build trajectory in max velocity mode v1.vac = 28000000 //max acceleration v1.vdc = 28000000 //max deceleration v1.vsp = 50000 v1.vse = 0
//max. velocity //end velocity

pi = 3.14159265358979 a = 100000 //ellipse axis

b = 50000 //ellipse axis b Xc = 0 // ellipse center coordinate by X Yc = 0 // ellipse center coordinate by Y v1.splines() // start spline trajectory for teta = 0:pi/18:2*pi x = Xc + a*cos(teta) y = Yc + b*sin(teta) v1.splinep(x,y) // add point to the spline trajectory end for v1.splinee(0) // end spline trajectory v1.bg
//start motion

After homing drives must at the first point of the sequence. Example Example (Motion Mathematic Lib Samples\ Vector_2D \ Lissajous curves www.elmomc.com) v1.vac = 28000000 //max vector acceleration v1.vdc = 28000000 //max vector deceleration v1.vsp = 100000 v1.vse = 0 v1.splines() //set max. velocity //set end velocity // start spline sequence

Maestro Motion Library Tutorial


MAN-INTUG (Ver. 1.7)

for t = 0:pi/72:2*pi x = R*cos(3*t) y = R*sin(5*t) v1.splinep(x,y) // add spline point end for v1.splinee(0) // end spline sequence

v1.bg //start motion while (a1.ms==2)||(a2.ms==2) //wait until both axes have stopped wait(10) end while

Figure 1-1: Recording of the spline trajectory (Lissajous curves)

1.3.3.2 Examples of three-dimensional spline interpolation


For 3D vector motion, there must be a defined resource file with a vector built from three axes. In the Elmo Studio it can be defined as shown in the picture below.

Maestro Motion Library Tutorial


MAN-INTUG (Ver. 1.7)

Figure 1-2: Resources for the 3D vector

Example (Motion Mathematic Lib Samples\ Vector_3D \ Spline_3D www.elmomc.com) A spline curve throws a number of arbitrary points SetAxisStartPos(a1, 0) //set coordinate x to 0 SetAxisStartPos(a2, 0) //set coordinate y to 0 SetAxisStartPos(a3, 0) //set coordinate y to 0 v2.vsp = 50000 v2.vse=0 v2.splines() // start spline sequence v2.splinep(0, 0, 0) // add spline 3D point v2.splinep(50000, 100000, 150000) // add spline 3D point v2.splinep(100000, 50000, 100000) // add spline 3D point v2.splinep(200000, 150000, 50000) //add spline 3D point v2.splinee(0) // end spline sequence Three-dimensional picture can be drawn in Matlab with the use of the following Matlab operators [n, posX, velX, posY, velY, posZ, velZ, t] = textread('D:\Dir_22_01\trj_file', '%d %d %d %d %d %d %d %d ', -1) where 'D:\Dir_22_01\trj_file full path to the PVT table file.

Maestro Motion Library Tutorial


MAN-INTUG (Ver. 1.7)

For this operator to work properly, the first line of the PVT table containing a text header must be removed. plot3(posX,posY,posZ) axis square; grid on

Figure 1-3: Three-dimensional picture corresponding to the calculated PVT table (drawn in Matlab) Results of the recording

Figure 1-4: Projection on the XY plane

Maestro Motion Library Tutorial


MAN-INTUG (Ver. 1.7)

Figure 1-5: Projection on the XZ plane

Example (Motion Mathematic Lib Samples\ Vector_3D \ Helix www.elmomc.com) Helix curve built with the use of the spline interpolation. SetAxisStartPos(a1, 50000) //set coordinate x to 0 SetAxisStartPos(a2, 0) //set coordinate y to 0 SetAxisStartPos(a3, 0) //set coordinate y to 0 v2.vsp = 50000 v2.vse=0 alpha = 0 //start angle degrees

beta = 3600 // sweep angle degrees R = 50000 H = 250000 // height X = 50000 Y=0 Z=0 n = 120 // start pos x // start pos y // start pos z // num points //radian

Teta = pi*(alpha/180)

dTeta = (pi*(beta/180))/n // teta increment for one step dZ = H/n Xc = X - R*cos(Teta) //z increment for one step // X coordinate of the helix axis

Maestro Motion Library Tutorial


MAN-INTUG (Ver. 1.7)

10

Yc = Y - R*sin(Teta) v2.splines() for i=0:1:n v2.splinep(X,Y,Z) Teta = Teta + dTeta X = Xc + R*cos(Teta) Y = Yc + R*sin(Teta) Z = Z + dZ end for v2.splinee(0) v2.bg

// X coordinate of the helix axis // start spline sequence

// add spline 3D point // calc teta for the next point // calc X coordinate for the next point // calc Y coordinate for the next point // calc Z coordinate for the next point

// end spline sequence // begin motion

Figure 1-6: Helix three-dimensional picture for the calculated PVT table drawn in MATLAB

1.3.4 Polyline
To build a polyline, the Maestro user program applies the following calls to the motion library functions: vector_name.starts(trj_name) starts the polyline trajectory sequence with saving the PVT table built by the Motion Library in a file named trj_name. Parameter trj_name can be missed in this case trajectory is saved in a temporary file named vector_name.trj. vector_name.ends() - ends the polyline trajectory sequence.

Maestro Motion Library Tutorial


MAN-INTUG (Ver. 1.7)

11

Inside the polyline operator parenthesis vector_name.starts(trj_name) and vector_name.ends() can be added function calls addline(), addcircle(), addsplinep() and adddwell() to define polyline segments. For the 2D polyline vector_name.addline (int PosX, int PosY) adds a line segment PosX, PosY - destination position of the linear segment (counts) vector_name.addcircle(int Radius, float StartAngle, float SweepAngle) adds circle arc segment Radius - radius of the circle segment (counts) start_angle - start angle of the circle segment (degrees) sweep_angle - sweep angle of the circle segment (degrees) vector_name.addsplinep(int x, int y) - adds two-dimensional spline point Important note: The user should take into account that the last point of the previous segment or the first point of the trajectory is automatically added to the spline. The minimal number of the addsplinep() operators that define the same spline segment inside the polyline must be great equal 2. This requirement is valid for every spline segment inside the polyline not only for the first one. For the 3D polyline vector_name.addline (int PosX, int PosY, int PosZ) adds a line segment PosX, PosY, PosZ - destination position of the linear segment (counts). vector_name.addsplinep(int x, int y, int z) - adds three-dimensional spline point As in a two-dimensional case, the last point of the previous segment is automatically added to the spline segment and the number of points defining the spline segment cannot be less than 2. For 2D and 3D polyline vector_name.adddwell(delay_time) adding delay (station) between two segments delay_time - delay value in millisecond Smooth transition from one curve to another inside polyline. There are four modes that define transition from one shape to another that are defined by the parameter (vector property) vsc:

1. 2.

vsc = 0 - switch arc is not built vsc = 1 ML builds switch arc with switch radius minimally radius that satisfies kinematics constraint r > (vse)2/[vac*vae] where vse parameter that defines end velocity on the segment preceding switch arc vac parameter that defines maximum vector acceleration vae parameter that defines admissible error for the vector acceleration

Maestro Motion Library Tutorial


MAN-INTUG (Ver. 1.7)

12

3. 4.

vsc = 2 ML builds switch arc with the switch radius vsr (this parameter must be set by the user). vsc = 3 - ML builds a switch arc with the switch radius implicitly pre-defined via parameter vsd (distance along the line from the intersection point). Parameter vsd must be set by the user.

For vsc = 2 and vsc = 3, the user can check if the values of the parameters vsr and vsd satisfy geometric constraints. Such a check can be done with the use of algorithms described in chapter 2 of this document. Switch arc building is also influenced by the previous segment end velocity defined by the parameter (vector property) vse.

1.3.4.1 Examples for the two-dimensional polyline


Example (Motion Mathematic Lib Samples\ Vector_2D \ LineCircle www.elmomc.com) v1.vac = 28000000 v1.vdc = 28000000 v1.vum = 1 v1.starts() v1.vsp = 50000 v1.vse = 50000 //max vector acceleration // max vector deceleration // build trajectory in max velocity mode // begin polyline trajectory // max velocity for the line segment // end velocity // request to add line shape // end velocity for the circle segment // smooth intersection with fixed switch radius // switch radius

v1.addline(100000, 100000) v1.vse = 0 v1.vsc = 2 v1.vsr = 10000

v1.addcircle(50000,225,180) //request to add circle arc shape v1.ends() // ends polyline trajectory

Maestro Motion Library Tutorial


MAN-INTUG (Ver. 1.7)

13

Figure 1-7: Recording of the polyline trajectory Any trajectory generated by the Motion Library (single shape or polyline) can be rotated relative to init point due to vector property vra rotation angle in degrees. Example (Motion Mathematic Lib Samples\ Vector_2D \ LineCircleRotated www.elmomc.com) // The same polyline rotated at 90 degrees relative to init point v1.vac = 28000000 v1.vdc = 28000000 v1.vum = 1 v1.vra = -90 v1.starts()
v1.vsc = 2 v1.vsp = 50000 v1.vse = 50000

//max vector acceleration // max vector deceleration // build trajectory in max velocity mode // rotate spline 90 degrees relative to init point // begin polyline trajectory
// smooth intersection with fixed switch radius // max velocity for the line segment // end velocity

v1.addline(100000, 100000) // request to add line shape v1.vsp = 50000 v1.vse = 0 v1.vsr = 10000 // maximum velocity for the circle segment // end velocity for the circle segment // switch radius

v1.addcircle(50000,225,180) //request to add circle arc shape v1.ends() // end polyline trajectory

Maestro Motion Library Tutorial


MAN-INTUG (Ver. 1.7)

14

Figure 1-8: Recording of the rotated polyline

1.3.4.2 Examples for the three-dimensional polyline


Example (Motion Mathematic Lib Samples\ Vector_3D \ Rectangle www.elmomc.com) Three-dimensional rectangle SetAxisStartPos(a1, 0) SetAxisStartPos(a2, 0) SetAxisStartPos(a3, 0) v2.vsc=2 v2.vsr=12000 v2.vsp = 70000 v2.vse = 70000 v2.starts() v2.addline(50000, 50000, 0) //create line from current point to coordinate v2.addline(50000, 50000, 50000) v2.addline(0, 0, 50000) v2.vse = 0 v2.addline(0, 0, 0) v2.ends() //max. vector velocity //set coordinate x to 0 //set coordinate y to 0 //set coordinate y to 0

Maestro Motion Library Tutorial


MAN-INTUG (Ver. 1.7)

15

Figure 1-9: Three-dimensional polygon drawn in Matlab

Results of the recording

Figure 1-10: Projection on the XZ plane

Maestro Motion Library Tutorial


MAN-INTUG (Ver. 1.7)

16

Figure 1-11: Projection on the YZ plane

1.4 Transition to a new trajectory with a non-zero velocity


If a transition from one trajectory to another (for instance from a line to a circle) must be executed with a velocity not equal to zero at the switch point, the Motion Library builds a switch curve to achieve smooth modification of the velocity. Such a curve is implemented as a circle arc. As a geometrical object switch arc is completely defined by: radius r coordinates of the two limit points (Xlast, where (Xlast, (Xfirst,

Ylast) and (Xfirst, Yfirst)

Ylast) last point on the first shape Yfirst) first point on the second shape

center coordinates (Xo,Yo)

The radius r of the switch arc can be: 1. Explicitly pre-defined by the user (vsc = 2, vsr defined). In this case, it must obey the following restriction:

r (Vend) 2/ACv
where Vend - end velocity at the switch point,

(1-1)

ACv - vector acceleration.

Maestro Motion Library Tutorial


MAN-INTUG (Ver. 1.7)

17

In fact, the value defined as the calculations.


2.

r (vse) 2/(vae*vac )

(by default vae = 0.9) must be used in

Implicitly pre-defined by the user via smooth distance d along the line from the intersection point (vsc = 3, vsd defined). This mode can be implemented to the line line, circle line and line circle trajectory intersections.

3. Calculated as a minimal possible for the given end velocity Vend and vector acceleration

ACv

(vsc = 1).

Regardless of the r definition (pre-defined by the user or calculated by (1-1)) movement along the switch arc is considered to be uniform with the constant tangent velocity V angle velocity

= Vend and constant


(1-2)

= V/r

The intersection geometry imposes constraints on the switch arc radius. So the switch radius in use

r must satisfy (Vend) 2/ACv < r < rmax


the switch radius limit value rmax. As a base value for the switch velocity is taken the preceding segment end velocity Vend defined by the parameter vse. This value is considered as a limiting parameter that cannot be exceeded but can be decreased. Its also valid in case that polyline segments (preceding switch arc and the following) are executed in the fixed velocity mode (vum = 3). In switch mode vsc = 1, the initial value of a switch velocity equal vse and can be decreased to build a switch arc trajectory equal to the integer number of PVT steps or milliseconds. In switch mode vsc = 1, the radius of the switch arc is calculated as a minimal possible for the intersection geometry and given vector acceleration/deceleration (input parameters vac/vdc) meaning that the calculated value satisfies (1-3). While the switch arc calculation is also taken into account the requirement to build switch arc trajectory equal to an integer number of PVT steps. The Motion Library is trying to build switch arc trajectory with velocity as close to the value given by the parameter vse for the preceding segment as possible. The switch arc (or two arcs on both ends of the segment) can take up almost all segment length that sometimes makes trajectory calculation impossible. As an example consider the following: segment initial velocity equal Vo, end velocity Ve and

(1-3)

Chapter 2 will consider all cases of the two shapes intersections and methods for the calculation of

min(Vo,Ve)T > L

(1-4)

(where L the length of the segment truncated by switch arcs, T standard PVT step). When defining switch radius (input parameter vsr) for vsc = 2 mode or end velocity (input parameter vse) for vsc = 1 mode, the user must take into account that it can significantly influences the whole trajectory and in fact replaces the main part of a particular segment by switch arcs.

Maestro Motion Library Tutorial


MAN-INTUG (Ver. 1.7)

18

Input parameters and intersection geometry define the influence of a switch arc on a trajectory. The main cases of shapes intersection are considered below. Here as an example to consider two lines intersection. If an angle between two lines is small, even a switch arc with a small radius can significantly change initial trajectory while an arc with the same radius can be insignificant for lines with intersection angle close to 180 . In addition to geometric constraints, the Motion Library imposes limitations on the switch arc length. Each switch arc should not exceed 50% of the segment length. If there are two switch arcs adjacent to some polyline segment, then both arcs should not take more than 80% of its initial length.
o

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-1

Chapter 2: Switch Radius Calculation


2.1 Line line intersection
If a trajectory contains a switch from line L1 to line L2 with non-zero velocity, the direction of the vector velocity cannot be changed at the one intersection point. To implement such a switch, insert into a trajectory an additional element circle arc is inserted into a trajectory (see-Figure 2-1). The switch arc radius must satisfy (1-1).

Figure 2-1 In the case of a line-line intersection, parameters r and equation

d are connected by the simple


(2.1-1

r = d*tg(/2)
where

= ( - angle between vectors L1 and L2) so the pre-defined parameter vsd = d must satisfy vsd (Vend)2/[ACv tg(/2)]
(2.1-2) If L1 is the length of the line L1 and L2 is the length of the line L2, then there is an evident geometric constraint for the switch radius r

rmax min(L1, L2)*tg(/2)


(2.1-3) In fact, due to additional limitations (50% of the segment length) used in ML, the following should be used

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-2

vsr min(0.5L1, 0.5L2)*tg(/2)


(2.1-4) and kinematics constraint

Vend [r*vae*ACv)]1/2
(2.1-5) where vae acceleration error (default value 0.9). So the user defined parameters must satisfy

vse [vsr*vae*vac]1/2
(2.1-6)

vse segment end velocity, vsr switch radius, vac vector acceleration.
Example 2.1a
Line 1 is defined by its init point (50000, 70000) and end point (60000,20000). .Line 2 is defined by the init point (60000,20000) and its end point (60000,70000). Switching from Line 1 to Line 2 must be executed with a minimal switch radius (vsc = 1). The cruise velocity is defined as vsp = 50000 and the end velocity vse = 50000. Vector acceleration/deceleration vac = vdc = 500000 1. The calculated minimal switch radius that satisfies kinematics constraint is r_min = (vse)2/(vac*vae) = (50000)2/(500000*0.9) = 5555.6 2. The calculated distance from the intersection point that corresponds to r_min = 5555.6

X1 = 60000 - 50000 = 10000, dY1 = 20000 70000 = -50000, X2 = 60000 60000 = 0, dY2= 70000 20000 = 50000 L1 = [dX12 + dY12]1/2 = [(10000)2 + (-50000)2]1/2 = 50990 L2 = [dX22 + dY22]1/2 = [0 + (50000)2]1/2 = 50000

= arcos[(X1X2 + Y1Y2)/(L1L2) = = arcos{[(-50000)*(0) + (-50000)*(50000)]/(50990*50000)} = 0.1974


The distance from the intersection point corresponding to the minimal switch radius

d = r_min/tg(/2) = 5555.6/tg(0.5*0.1974) = 56105 d > L1 and d > L2


which means that the minimal switch radius does not fit the geometric constraints. Possible solutions: to decrease the end velocity vse or increase vector acceleration vac. Suppose that the vector acceleration is pre-defined by the mechanical parameters of the system and decrease end velocity.

dmax = min(L1,L2) = 50000

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-3

r_max = dmax*tg(/2) = 50000* tg(0.5*0.1974) = 4951


This value is limiting and produces singular trajectory with the switch arc that replaces

L2. So we should take some value less than limiting value 4951 for instance 0.5* r_max =
0.5*4951 = 2475. Now we can recalculate maximum end velocity (vse) that satisfies this value:

vse = [r*vac*vae]1/2 = [2475*500000*0.9]1/2 = 33376


(any value less equal than 33376 can be used with the switch radius r = 2475).

Example 2.1b
Line 1 is defined by init point (50000, 70000) and end point (60000,20000). Line 2 is defined by the init point (60000,20000) and its end point (60000,70000). Switch from Line 1 to Line 2 must be executed with the pre-defined switch radius (vsc = 2). We define cruise velocity vsp = 50000 and end velocity vse = 50000. Vector acceleration/deceleration vac = vdc = 500000 and switch radius vsr = 6000 1. We calculated minimal switch radius that satisfies the kinematics constraint r_min = (vse)2/(vac*vae) = (50000)2/(500000*0.9) = 5555.6 Pre-defined switch radius is greater than r_min so it satisfies kinematics constraints 2. We have to check if switch radius r_switch = 6000 satisfies geometric constraints

X1 = 60000 - 50000 = 10000, dY1 = 20000 70000 = -50000, X2 = 60000 60000 = 0, dY2= 70000 20000 = 50000 L1 = [dX12 + dY12]1/2 = [(10000)2 + (-50000)2]1/2 = 50990 L2 = [dX22 + dY22]1/2 = [0 + (50000)2]1/2 = 50000

= arccos[(X1X2 + Y1Y2)/(L1L2) =
=

arccos{[(-50000)*(0) + (-50000)*(50000)]/(50990*50000)} = 0.1974

The distance from the intersection point that corresponds to the r_switch = 6000

d = r_switch/tg(/2) = 6000/tg(0.5*0.1974) = 60593


The calculated value exceeds L1 and L2 meaning that the chosen switch radius does not fit geometric constraints and must be decreased:

r_max = min(L1, L2)tg(/2) = 50000*tg(0.5*0.1974) = 4951.


It is possible to choose any value r_switch that satisfies r_min < r_switch and r_switch < r_max but in this case r_min > r_max. So as r_switch, use 0.9min(r_min , r_max) = 4455.9. The chosen value exceeds r_min so it does not fit the kinematics constraints and the end velocity must be decreased:

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-4

vse = [r_switch*vac*vae]1/2 = [4455.9*500000*0.9]1/2 = 44778.9


Example 2.1c (Motion Mathematic Lib Samples \Line To Line\LineLineMinRad_Ex_2c www.elmomc.com) Line 1 is defined by its init point (300000, 900000) and end point (700000,200000). Line 2 is defined by the init point (700000,200000) and its end point (1100000,700000). Switch from Line 1 to Line 2 must be executed with the minimal switch radius (vsc = 1). We define the cruise velocity vsp = 50000 and the end velocity vse = 50000. Vector acceleration/deceleration is vac = vdc = 28000000. 1. We calculate the minimal switch radius that satisfies kinematics constraint by r_min = (vse)2/(vac*vae) = (50000)2/(28000000*0.9) = 99.2 2. Geometric description of the two lines intersecting:

X1 = X12 X11 = 700000 - 300000 = 400000 Y1 = Y12 Y11 = 200000 900000 = -700000 X2 = X22 X21 = 1100000 700000 = 400000 Y2 = Y22 Y21 = 700000 200000 = 500000
The length of the first line segment

L1 = [X12 + Y1 2]1/2 = [4000002 + (-700000) 2]1/2 = 806225.8


The length of the second line segment

L2 = [X22 + Y12]1/2 = [4000002 + 5000002]1/2 = 640312.4


An angle between two lines can be calculated as

= arccos[(X1X2 + Y1Y2)/(L1L2) = = arccos{[400000*400000 + (-700000)( 500000)]/(806225.8*640312.4)}=


1.193887 = 68.4o
The distance from the intersection point corresponding to the minimal switch radius

d = r_min/tg(/2) = 99.2*tg(0.5*1.193887)= 145.95


We see that d constraints.

< L1 and d < L1 so the calculated switch radius satisfies the geometric

Example 2.1d
(Motion Mathematic Lib Samples\Line To Line\LineLineFixedDist_Ex_2d www.elmomc.com)

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-5

Line 1 is defined by its init point (300000, 900000) and end point (700000,200000). Line 2 is defined by the init point (700000,200000) and its end point (1100000,700000). Switch from Line 1 to Line 2 must be executed with a distance from the intersection point pre-defined by the user vsc = 3, vsd = 20000. We define cruise velocity vsp = 50000 and end velocity vse = 50000. Vector acceleration/deceleration vac = vdc = 28000000 1. We calculate the minimal switch radius that satisfies the kinematics constraint r_min = (vse)2/(vac*vae) = (50000)2/(28000000*0.9) = 99.2 2. Geometric description of the two lines intersecting:

X1 = X12 X11 = 700000 - 300000 = 400000 Y1 = Y12 Y11 = 200000 900000 = -700000 X2 = X22 X21 = 1100000 700000 = 400000 Y2 = Y22 Y21 = 700000 200000 = 500000
The length of the first line segment

L1 = [X12 + Y1 2]1/2 = [4000002 + (-700000) 2]1/2 = 806225.8


The length of the second line segment

L2 = [X22 + Y12]1/2 = [4000002 + 5000002]1/2 = 640312.4


Notice that d < L1 and d < L1 An angle between the two lines can be calculated as

= arcos[(X1X2 + Y1Y2)/( L1L2)] = = arcos{[400000*400000 + (-700000)( 500000)]/(806225.8*640312.4)}= 1.193887 = 68.4o


The switch radius corresponding to the given d can be calculated as

r=d*tg(/2) = 20000tg(0.5*1.193887) = 13593

1.

Check the kinematics constraints. A minimal switch radius that fits kinematics constraints can be calculated as

rmin > vse*vse/(vac*vae) = 500002/(28000000*0.9) = 99.2


The implicitly pre-defined switch radius is 13593 >> rmin. A number of similar examples with a different set of user defined and calculated parameters are considered in the Motion Mathematic Lib Samples examples TwoLinesFixDist_Fx2D_1 - TwoLinesFixDist_Fx2D_2.

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-6

2.2 Circle line intersection


Note: C circle arc, L line, R circle radius, r switch arc radius, (Xc,Yc) - circle center, (Xi, Yi) intersection point, (Xlast, Ylast) last point on the circle (Xfirst, Yfirst) first point on the line L, d distance from point (Xi, Yi) to point (Xfirst, Yfirst), (Xo,Yo) switch arc center coordinates. There are three possible cases that influence the calculation of parameters that define a switch arc: initial circle center and switch arc center belong to the same half-plane, initial circle center and switch arc center belong to different half-planes (defined by the line L), and when the line (or continued line) moves through the center of the initial circle. For each of these three cases, two sub cases are possible: from the point of intersection the line goes either outside or inside the circle.

Circle line intersection geometry must satisfy some necessary conditions for the switch arc to be built.
On the first stage of calculations we define switch arc radius. It can be predefined by the user or calculated as V

/(vae*ACv), where V end velocity and ACv vector acceleration, vae -admissible acceleration error. If the intersection was defined by the distance d than the
2

switch arc radius can be calculated as described in 2.2.2 No matter how a switch arc radius was defined it must be coordinated with a circle and a line parameter

2.2.1 Line goes inside the circle

2.2.1.1 Switch arc center and circle center belong to two different half planes defined by the line L
The switch arc radius must obey

(vse)2/(vac*vae) < vsr < (R h)/2


where R - radius of the initial circle, h acceleration error.

(2.2.1.1-1)

the length of the perpendicular dropped from the circle center on the line L, vsr - parameter that defines switch arc radius, vae admissible
This condition is necessary but not always sufficient. Its sufficient only in the case that the projection point P1 of the circle center on the line belongs to the line segment (P1 Figure 2-2) belongs to the circle arc (P2

L)

and the point of intersection of the continued perpendicular with the circle (point P2 in

C).

If one of these conditions is not fulfilled, the maximum possible switch radius r must be recalculated due to intersection geometry. Possible cases are considered below.

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-7

Figure 2-2

Example 2-2 (Motion Mathematic Lib Samples\Circle to Line\ Section 2_2_1_1\ CircleLine_Ex_2_2 www.elmomc.com) Circle arc is defined by its init position Xfrom = 0, Yfrom = 100000, radius R = 100000, init angle = 90o(with axis X positive direction), and sweep angle = 180o. Line end point Xto = -140000, Yto = 100000. Circle center coordinates Xc = Xfrom Rcos(/180) = 0 100000cos(/2) = 0 Yc = Yfrom Rsin(/180) = 100000 100000sin(/2) = 0 Circle end points X1 = Xc + Rcos( + ) = 0 + 100000cos(/2 + ) = 0 Y1 = Yc + Rcos( + ) = 0 + 100000sin(/2 + ) = -100000 Projection of the circle center on the line (Appendix ) K1 = (Y2 Y1)/(X2 X1) = (100000+100000)/(-140000-0)=-1.4286 K = -1/K1 = 0.7 Xp=(Yc Y1 + K1*X1 - KXc)/(K1 - K) = (0+100000 + 0.7*0 +1.4286*0))/(-1.4286 - 0.7) = -46979

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-8

Yp = Yc + K*(Xp Xc) = 0 +0.7*(-46979 - 0) = -32885 And the perpendicular length h = [(Xc Xp)2 + (Yc - Yp)2]1/2 = [(0 + 46979)2 + (0 + 32885)2]1/2 = 57345 Max switch radius calculated by (2.2.1.1-1) Rmax = (R h)/2 = (100000 57345)/2 = 21328 Rmin = 1000002/(2800000*0.9) = 396.8 As a switch radius we can take any value Rswitch that satisfies Rmin < Rswitch < Rmax We take Rswitch = (Rmin + Rmax)/2 = (21328 + 396.8)/2 = 10862

In Figures 2-3 and 2-5, two possible cases are presented when the projection of the circle center on the line L does not belong to the line segment. In Figure 2-3, perpendicular at the Line L end point P2 intersects with the circle

P3 that belongs to the circle arc P3 C. In Figure 2-3, observe the switch arc with the maximum possible radius r. This limit value can be calculated.

C at point

Figure 2-3 Since the coordinates of point P1(X1,Y1) projection of the circle center point

O(Xc,Yc) on line L and line end point P2(X2,Y2) are known, the distance is (P1,P2) = u = [(X2 X1)2 + (Y2 Y1)2]1/2
(2.2.1.1-2)

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-9

The length of the perpendicular h should also be calculated. By knowing the line equation in a form

Ax + By + C = 0
then h can be calculated by

(2.2.1.1-3)

h = |AXc + BYc + C|/(A2 + B2)1/2 A,B,C can be calculated by formulas in Appendix 2 or Appendix 1.
To define r we can use an equation

(2.2.1.1-4)

If line L is defined by its starting point (X1,Y1) and end point (X2,Y2), coefficients

(R r)2 = (h + r)2 + u2 R2 2Rr + r2 = h2 + 2hr + r2 + u2 rmax = (R2 h2 u2)/(2R + 2h)

(2.2.1.1-5) (2.2.1.1-6) (2.2.1.1-7)

This means that the parameter vsr that defines switch arc radius must obey the following constraints:

(vse)2/vac < vsr < (R2 h2 u2)/(2R + 2h)


(2.2.1.1-8) The switch arc with parameter vsr calculated by (2.2.1.1-7) takes up the whole line segment. To satisfy additional requirement that switch arc cannot take more than 50% of the segment length instead of the line end point we should use in calculations some other point P2 that belongs to the line segment. The distance between this point and the intersection point

2 =

(Pi,P2) must be equal k*l (where k < 0.5 , l the length of the line). If the distance from the intersection point Pi to P1 is defined as (Pi P1), then (2.2.1.1-7) can be presented as a
function of the used partial length k*l of the line L.

rmax = [R2 h2 ((Pi P1) k*l)2]/(2R + 2h)


(2.2.1.1-9)

Example 2-4 (Motion Mathematic Lib Samples\Circle to Line\ Section 2_2_1_1\ CircleLine_Ex_2_4 www.elmomc.com)
Circle arc (Figure 2-4) is defined by its init position Xfrom = 0, Yfrom = 0, radius R = 100000, init angle = 90o(with axis X positive direction), and sweep angle = +60o. Line end point Xto = 25000, Yto = 50000. Circle end point [Xc + Rcos(-30), Yc + Rsin(-30)] = (86603, 50000) so the line L is parallel to the axis X. From Figure 2-4, h = 50000, u = 25000. The switch radius is calculated by (2.2.1.1-7).

r = 0.5*(1000002 500002 250002)/(100000 + 50000) = 22917

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-10

Figure 2-4

In our calculations was not taken in account additional requirement that the switch arc should not take more than 50% of the segment length. So the calculated switch arc with the radius 22917 takes the whole line segment. If in the calculations, some point on the line is used, for example (65000, 50000), instead of the line end point, then u = 65000 and the result is:

r = 0.5*(1000002 500002 650002)/(100000 + 50000) = 10917


Perpendicular to the Line L end point P2 does not intersect the circle arc Line L end point P2 does not intersect the circle arc is a switch arc with a maximum possible radius

In Figure 2-5, there is an intersecting geometric figure showing that a perpendicular line at

C. The switch arc drawn in Figure 2-5

r.

P6(X6,Y6) it is possible to find an intersection point P3 of two lines: (Xc,Yc) (X6,Y6) and L and the length of the line segment P3P6: 1 = (P3, P6).
Drop a perpendicular from point

By knowing the coordinates of a circle center O(Xc,,Yc) and a circle init point

P6 on the line L and by (2.2.1.1-4) define its length

2 = (P6, P4).
(2.2.1.1-9) and finally

To define

r, use a proportion

r/2 = (1 r)/1

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-11

r = 12/(1 + 2)
As in the previous case, the user defined parameter vsr must obey

(2.2.1.1-10)

(vse)2/vac < vsr < 12/(1 + 2)

(2.2.1.1-11)

Figure 2-5

Example 2-6

(Motion Mathematic Lib Samples\Circle to Line\ Section 2_2_1_1\ CircleLine_Ex_2_6 www.elmomc.com) The circle arc in Figure 2-6 is defined by the radius R = 100000, init angle = -90o, sweep angle = 30o and the init position Xfrom = 0, Yfrom = -100000. Line L end point: Xto = -40000, Yto = 0; The circle arc end point P5 coordinates can be calculated as X5 = R*cos(/3) = 50000, Y5 = R*sin(-/3) = -86602.54; Line L is defined by two points P1(-40000, 0) and P2[Rcos(-60), Rsin(-60)] = P2(50000, -86602.54). Its standard equation is Ax + By + C = 0,
where A = (Y2 Y1) = -(0 + 86602.54) = 86602.54, B = X2 X1 = -40000 50000 = -90000, C = X1(Y2 Y1) Y1(X2 X1) = 50000(0 + 86602) + 86602.54(-40000 50000) =3464101600.0 where (X1, Y1) and (X2,Y2) line end points.

Calculate 2 by (3.1.1-4) as 2 = |(-86602.54)0 + (-90000)(-100000) - 3464101600.0|/[(-86602.54)2 +(-90000)2 ]1/2 = 44322.6596 To define 1, take into account that line (O, P6) in Figure 2-6 coincides with the axis Y so coordinates of its intersection point with the line L are (0, -C/B).

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-12

1= 100000 - |C/B| = 100000 - |(-3464101600.0)/(-90000)| = 61509.98222 r = 12/(1 + 2) = 61509.98*44322.66/(61509.98 + 44322.66)= 25760.351


meaning that the switch radius in use must be less than this limiting value.

Figure 2-6

Values calculated by (2.2.1.1-7) and (2.2.1.1-10) are not recommended but limiting values. The switch radius in use must be less those values. Such limiting values produce irregular cases of intersection. If we use (2.2.1.1-7) switch arc replaces line L and if we use (2.2.1.1-9) switch arc replaces initial arc C.

2.2.1.2Switch arc center and a circle center belong to the same half plane.
In this case (Figure 2-7), the switch radius must satisfy the following condition:

r (R + h)/2

(2.2.1.2-1)

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-13

Figure 2-7 This condition is not always sufficient. Adequacy depends on arc and line lengths. If the circles center projection on the line L belongs to the line segment (P1 L) and an intersection point of the continued perpendicular with the circle arc belonging to the circle segment P2

C then (2.2.1.2-1) is sufficient.

If projection of the circle init point P1 belongs to the line segment - P5 8), use the following proportion

L (Figure 2(2.2.1.2-2)

h/r = (P3, Oc)/[(P1, P3) r] = 1/(2 r)


where P3 is the point of intersection of the line (P1,Oc) with the line

L. As init point of the circle P1 and a circle center are known, P3 can be determined. The length h of the perpendicular dropped from the circle center Oc on the line L can be also found by
(2.2.1.2-4) or another method. From (2.2.1.2-2) the results are

h2 hr = r1 => r = h2/(1 + h)

(2.2.1.2-3)

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-14

Figure 2-8 Example 2-9

(Motion Mathematic Lib Samples\Circle to Line\ Section 2_2_1_2\ CircleLine_Ex_2_9 www.elmomc.com)


The circle arc (Figure 2-9) is defined by its radius R = 100000, init angle angle

= 0o and sweep

= -45o, init point P1(100000, 0). Line target position (-40000, 0). 2 = 140000 - line (P1,P3) coincide with axis X.

In this case 1 = 40000,

Circle center coordinates Xc = 100000 - Rcos(0) = 0, Yc = 0 - Rsin(0) 0. Line init point coordinates X1 = 0 + 100000cos(0 - 45) = 70711, Y1 = 0 + 100000sin(0-

45) = -70711
To calculate the length of perpendicular h dropped from the circle center (Xc,Yc) on the line, use formulas (a1.6), (a1.4) and (a1.7) from Appendix 1.

k = (Y2-Y1)/(X2-X1)=(0+70711)/(-40000-70711)= -0.6387, q = -1/k = 1.5657 Xpr = (y - Y1 + k*X1 - q*x)/(k - q) = -11589.6, Ypr = y + q*(Xpr - x) = - 18145.7 h= [(-11589.6 - 0)2 + (- 18145.7 - )2]1/2 = 21531

r = h2/(1 + h) = (21531*140000)/( 40000 + 21531) = 48989

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-15

Figure 2-9 Projection of the circle arc init point P1 on the line

L does not belong to the line segment L.

L does not belong to the line segment L (Figure 2-10), calculate the maximum switch radius r using equation 2(Oc,P5) + (r h)2 = (R r)2
or (2.2.1.2-4)

If a projection of the circle arc init point P1 on the line

(1)2 + (r h)2 = (R r)2 (1)2 + r2 2rh + h2 = R2 2Rr + r2 R2 (1)2 h2 = 2Rr 2rh


The results for

(2.2.1.2-5) (2.2.1.2-6) (2.2.1.2-7)

r
(2.2.1.2-8)

r = [R2 (1)2 h2]/(2R 2h)

To define (Oc,P5), drop a perpendicular from the circle center Oc on the line

L. The distance from the projection point P4 to the line end point P3 is equal to (Oc,P5).

As well as in case when the circle and switch arc centers belong to different half planes values calculated by (2.2.1.2-3) and (2.2.1.2-8) are not recommended but limiting ones. Switch radius in use must be less those values.

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-16

Figure 2-10 Example 2-11

(Motion Mathematic Lib Samples\Circle to Line\ Section 2_2_1_2\ CircleLine_Ex_2_11 www.elmomc.com)


Circle arc (Figure 2-11) is defined by its init position (X = 0, Y = 60000), radius R = 60000, init angle

= 0o, and a sweep angle = -135o. Line target position 25000, -42426.

Circle center coordinates (Xc, Yc) = (0 Rcos90, 60000 Rsin90) = (0,0). Circle end o o point coordinates (Xc + Rcos(-45 ), Yc + Rsin(-45 ) = (42426, - 42426). Notice that line L is parallel to the axis X meaning that h = |0 - 42426| = 42426, =|25000 - 0| =25000 and r can be calculated by

(3.1.2-8): r = (60000*60000 25000*25000 42426*42426)/(2*60000 2*42426) = 33430

Figure 2-11

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-17

2.2.1.3 Line intersects the center of the circle


Consider the last case of the circle line intersection: the line goes inside the circle through the center of the circle (Figure 2-12-2-15). The following cases are possible: a) The length of the line L is greater than the circle radius R and the circle diameter orthogonal to the line intersects the circle arc: P1 C1 (Figure 2-12). In this case an evident geometric constraint on the switch arc radius is

r R/2

(2.2.1.3-1)

Figure 2-12 b) The length of the line L is less than the circle radius R and a perpendicular at the line end point intersects the circle arc:

P1 C1 (Figure 2-13). To define the

geometric limit for the switch radius r, use the following equation:

(R r)2 = (R L)2 + r2
(2.2.1.3-2) or

2Rr = 2RL + L2
that leads to

r = (2RL L2)/2R = L L2/(2R)

(2.2.1.3-3)

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-18

Figure 2-13 Example 2-14

(Motion Mathematic Lib Samples\Circle to Line\ Section 2_2_1_3\ CircleLine_Ex_2_14 www.elmomc.com)


The circle C1 (Figure 2-14) is defined by its init point (80000,0), radius R = 80000, init angle = 0 and a sweep angle = -135. Line target position (-28284, -28284). The length 1/2 of the line segment L = 28284*2 = 40000.

The circle C1 is defined by the init point (80000,0), radius R = 80000, init angle = 0
and a sweep angle

= 135.
2 1/2

Circle target position: Xend = 80000*cos(135) = - 56569, Yend = 80000*sin(-135) = -56569. The length of the line segment L = [(-56569 + 28284) * 2] Calculate the upper limit of the switch radius by (3.1.3-3): r

= 40000.

= (2RL L2)/(2R) =

(2*80000*40000- 400002)/(160000) =30000

Figure 2-14

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-19

c) The circle arc sweeps an angle less than 90o and a perpendicular dropped from the circle init point P1 on the line L intersects line segment at point P 2 15). A projection of the circle init point P1(X1,Y1) on the line L

C1 (Figure 2-

point P2(X2,Y2)

P2: 1 = (P3,P2) can be determined. Now calculate the maximum switch radius
with the use of the following equation:

and the length of the segment defined by the line init point P3 and projection point

(R r)2 = (R 1)2 + r2
(2.2.1.3-4) that leads to

2Rr = 2R1 + (1)2


and finally

r = [2R1 (1)2]/(2R)
(2.2.1.3-5)

Figure 2-15 Example 2-16

(Motion Mathematic Lib Samples\Circle to Line\ Section 2_2_1_3\ CircleLine_Ex_2_16 www.elmomc.com)


The circle (Figure 2-16 ) is defined by its init point P1(0, -80000), radius R = 80000, init angle = -90o and sweep angle =-45o. Coordinates of the line end point (150000,150000). Circle end point P3 (line init point) coordinates are calculated as X3 = 80000*cos(pi + pi/4) = 56569 and Y3 = 80000*sin(pi + pi/4) = -56569 Drop a perpendicular from the circle init point P1 on the line L2. To define coordinates of the projection point P2 , use formulas from the Appendix 1. k = dY/dX = (150000 + 56569)/ (150000 + 56569) = 1, q = 1/k = 1.

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-20

By (a1.6) we have

Xp = (Yo Y1 + kX1 qXo)/(k q) = (80000 + 56569 56569 - 0)/(1+1) =


40000 Yp = Yo

+ q(X Xo)

= -80000 (40000 - 0) = 40000.

Distance 1 =

(P3,P2) = [(40000 + 56569)2 + (40000 + 56569)2]1/2 = 23432

For the maximum switch radius we get from (3.1.3-5)

r = [2R1 (1)2]/(2R) = [2*80000*23432 - 234322]/160000 = 20000

Figure 2-16

2.2.2 Switch arc radius calculation by the distance from the intersection point
If svc = 3 mode (vsd = d is given) is considered and it is important to know the switch arc radius r to check if end velocity and vector acceleration satisfy (1-1). If d distance from the point (Xi,Yi) to the point (Xfirst,Yfirst) is given, then it can be useful to re-calculate r as a function of parameters d and R (we have to know r to check condition 1-1). Consider three possible cases of a circle and switch arc positions relative to the line.

2.2.2.1 Initial circle center and switch arc center belong to the same half-plane
2.2.2.1.1 Line continues outside the circle (Figure 2-17)
As in case of the switch arc center coordinates calculation we drop a perpendicular from the circle center (Xc,Yc) on the line and get a projection point (Xp,Yp). The length of the perpendicular 1 can be defined as

1 = [(Xp Xc)2 + (Yp Yc)2]1/2


Define point (X1,Y1) so that

(2.2.2.1.1-1)

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-21

[(Xp,Yp),(X1,Y1)] = r
(2.2.2.1.1-2)
The following is known:

3 = [(Xp,Yp),(Xf, Yf)] = [(Xp,Yp),(Xi, Yi)] + d


(2.2.2.1.1-3)
where (Xi, Yi) circle-line intersection point . As [(X1,Y1),(Xo,

Yo)] = [(Xp,Yp),(Xf, Yf)] = 3 then use an equation to define r

(R + r)2 (3)2 = (1 r)2


(2.2.2.1.1-4) From (2.2.2.1.1-4) for r there is

r = [(3)2 + (1)2 R2]/(2R + 21)


(2.2.2.1.1-5) 2.2.2.1.2 Line continues inside the circle (Figure 2-18)
The following is known:

3 = [(Xp,Yp),(Xi,Yi)] d
(2.2.2.1.2-1)

Figure 2-17

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-22

Figure 2-18 so an equation can be written

(R r)2 (1 r)2 = (3)2


that produces for r

(2.2.2.1.2-2)

r = [R2 (3)2 (1)2]/(2R 21)

(2.2.2.1.2-3)

2.2.2.2 Initial circle center and switch arc center belong to two half planes defined by the line L.
2.2.2.2.1 Line continues outside the circle (Figure 2-19)
In this case

3 = [(Xp,Yp),(Xi,Yi)] + d
(2.2.2.2.1-1)
Equation for r

(1 + r)2 = (R + r)2 (3)2


From (2.2.2.2.1-2) we have

(2.2.2.2.1-2)

r = [(1)2 + (3)2 R2]/(2R 21)

(2.2.2.2.1-3)

2.2.2.2.2 Line continues inside the circle (Figure 2-20)

3 = [(Xp,Yp),(Xi,Yi)] d
Equation for r

(2.2.2.2.2-1)

(R r)2 (3)2 = (1 + r)2

(2.2.2.2.2-2)

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-23

that produces

r = [R2 (1)2 (3)2]/(2R + 21)

(2.2.2.2.2-3)

Figure 2-19

Figure 2-20

Figure 2-21

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-24

2.2.2.3 Circle center (Xc,Yc) L1 (line L1 intersects the center of the circle)
2.2.2.3.1 Line goes outside of the circle (Figure 2.11).
An equation for r

(R + r)2 = (R + d)2 + r2 r = (2Rd + d2)/(2R)


2.2.2.3.2 Line goes inside the circle (Figure 2.12) (R r)2 = (R d)2 + r2

(2.2.2.3.1-1) (2.2.2.3.1-2)

(2.2.2.3.2-1) (2.2.2.3.2-2)

r = (2Rd d2)/(2R)

Figure 2-22

2.2.3

Line goes outside the circle

If the line goes outside of the circle, the switch arc radius is limited by the length of the line segment and the length of the circle arc segment. The connection between the switch arc radius and distance d between intersection point and a last point of the switch arc on the line L was considered in section 3.2. The maximum switch radius for the given line segment L can be calculated by formulas (2.2.2.1.1-5) or (2.2.2.2.1-3) from 2.2.2.1.1 where

d = L.
When considering the limitations imposed by the length of the circle arc (defined by the sweep angle and a circle radius), three cases are possible:

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-25

1. Circle init radius intersects with the line L continued in its positive direction (Figure 2-23); 2. Line L is parallel to the init radius (Figure 2-25); 3. Line L continued in the reverse direction intersects init radius (Figure 2-26) (by the init radius there a line segment that connects the circle arc center with the circle arc init point).

2.2.3.1 Line L and init radius continued in their positive intersecting directions
The init circle radius continuing in its positive direction intersects with the line L continued in its positive direction (Figure 2-23). It takes place if the circle arc init angle with the axis X positive direction is less than the line L angle with the axis X positive direction and the circle arc sweep angle

< 0 ( < 0, < ) or in case: > 0, > .

Figure 2-23

L. Its length h can be calculated by (2.2.1.1-4). Now calculate the angle = arccos(h/R), where R circle arc radius. The circle arc sweep angle so = + = + arccos(h/R).
Now calculate the distance

Drop a perpendicular from the circle center Oc on the line

d = (Oc, P2) = h/cos()


To define the switch radius r, use a proportion

(2.2.3.1-1)

r/h = (d R r)/d

(2.2.3.1-2)

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-26

or

rd = hd hR hr
and for r the result is

(2.2.3.1-3)

r = h(d R)/(d + h )
Example 2-24

(2.2.3.1-4)

(Motion Mathematic Lib Samples\Circle to Line\ Section 2_2_3_1\ CircleLine_Ex_2_24 www.elmomc.com)


The circle is defined by its radius R = 50000, init angle

= 45o and sweep angle = -45o

and the init point (35355, 35355). The line L end point is (80000, 80000). The circle end angle = 0o, so the line init point coordinates (50000, 0). For the coefficients A, B, C in the line standard equation Ax + By + C = 0,

A = -(Y2 - Y1) = -0 + 80000 = 80000 , B = X2 - X1 = 80000 50000 = 30000, C = X1(Y2 - Y1) - Y1(X2 - X1) = 50000(80000 0) 0 = 4000000000. The length of h
can be calculated as:

h = |C|/(A2 + B2)1/2 = 4000000000/(80000*80000 + 30000*30000)1/2 = 46816.5


The length h of the perpendicular can be also calculated by formulas (a.16),(a1.4) and (a.17) from Appendix1 (this approach is used in the sample program CircLine_ex_3_13a).

= 45o = /4 = 0.785398, = arccos(h/R) = arccos(46816.5/50000) =


0.358768.

= + = 0.785398 + 0.358768 = 1.144166. d = h/cos() = 46816.5/cos(1.144166) = 113136 and the for the switch radius: rmax = h(d R)/(d + h) = 46816.5(113136-50000)/(113136 + 46816.5) = 18479

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-27

Figure 2-24

2.2.3.2 Line parallel to the circle arc init radius


a) Line direction coincides with the direction of the init radius In this simple case (Figure 2-25) maximum switch radius is equal to the distance between line L and circle arc init radius. This distance is equal to length of perpendicular from the circle arc center (2.2.1.1-4).

h dropped

Oc on the line L. To define it we can use the same formula

Figure 2-2-25a
b) Line direction is opposite to the direction of the init radius

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-28

Figure 2-25b
Maximum switch radius is perpendicular to the line L at the line end point. h the length of the perpendicular dropped from the circle center on the line. We can calculate an angle between the perpendicular at the line end point and the line that connect the centers of the init circle and a switch arc

= 180 90 (180 ) = 90
To define max switch radius we can use an equation

(2.2.3.2-1)

(R + r)cos = r + h
that produces

(2.2.3.2-1)

r = (Rcos h)/(1 cos)

(2.2.3.2-1)

2.2.3.3 Line L and init radius continued in their reverse directions intersect
This case is shown in Figure 2-26. 1. Find the coordinates P1(X1,Y1) intersection point of two lines init circle radius

(Oc,P2) and a line L.


2. Knowing the coordinates of circle arc center Oc(Xc,Yc) and intersection point

P1(X1,Y1) makes 1 = (P1,Oc) = [(X1 - Xc)2 + (Y1 - Yc)2]1/2 known.

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-29

3. Know trajectory init point P2(X2,Y2), calculate 2 =

(p2, p1) =

[(X2 X1)2 + (Y2 Y1)2]1/2


4. Drop a perpendicular from the circle center Oc on the line L and calculate its length h by (2.2.1.1-4) 5. To get switch arc radius, use a proportion

1/(2 + r) = h/r
(2.2.3.3-1)

or

1r = h2 + hr
(2.2.3.3-2) and finally the value for r is:

r = h2/(1 - h)

(2.2.3.3-3)

Figure 2-25 Example 2-27 (Motion Mathematic Lib Samples\Circle to Line\ Section 2_2_3_3\ CircleLine_Ex_2_27 www.elmomc.com)
The circle arc is defined by its radius R = 40000, init angle = 30o and sweep angle = -30o, init point (34641, 20000) Figure 2-27. Line L target point (130000, 25000). Coordinates of the circle center Oc: Xc = 34641 40000cos(/6) = 0, Yc = 20000 40000sin(/6) = 0; Define the coordinates of the intersection point P1(X1, Y1). For X1 and Y1, use expressions

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-30

By (a3.6)-(a3.7) from Appendix 3. q1 = X1/Y1= (34641-0)/(20000-0) = 1.73205 Y1 = ((q1*Y1 + X21 X11)*Y2 Y21X2)/(q1*Y2 X2) = ((1.73205*0 + 40000 0)*(25000-0) - 0)/( 1.73205*(25000-0) - (130000-40000)) = -21414X1 = X11 + q1 (Y1 Y11) = X1 = 0 + 1.73205(-21414- 0) = -37090 To get the length of the perpendicular h from the circle center on the line L, line L is needed.

Figure 2-26
standard equation in a form Ax + By + C = 0. For A,B,C we have A = -(Y22 Y21) = 25000, B = 130000 40000 = 90000, C = C = X21(Y2 Y21) - Y1(X22 X21) = 40000(25000-0) = 109.

h = (0*25000 + 0*90000 + 109)/( 250002 + 900002)1/2 = 107061 = [(-37090)2 + (21414)2]1/2 = 42828

2 = [(-37090 34641)2 + -21414 20000)2]1/2 =82828


and for maximum switch radius we get

r = h2/(1 h) = 10706*82828/(42828 10706) = 27605 2.3 Circle circle intersection


Note: C1 first circle arc, C2 second circle arc, Pi point of two circle arcs intersection,

Co1 first circle center point, Co2 second circle center point.

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-31

2.3.1

One of two circle arcs intersects the internal area of the second

If the circle arc C1 comes from inside of the circle

C2 Figure 2-28 (or circle C2 continues

inside the circle C1), then the switch arc radius must satisfy the necessary condition

r (R2 + h)/2
(2.3.1-1) where h = (O2,P1) distance from the circle C2 center O2 to the intersection point connecting the centers of two circles with the circle arc C1 (Figure 2-28). Condition (2.3.1-1) is not always sufficient only in cases that the points of

P1(X1,Y1) of the line O1O2


intersection O1O2 with

P2(X2.Y2)

C1 and C2 belong to C1 and C2: P1(X1,Y1) C2 (Figure 2-28).

C1 and

Figure 2-28
In Figure 2-29 the case when a point of intersection of the line O1O2 does not belong to the circle arc C1 is presented. Line O1P1 goes through the circle C1 center O1 and its init point P1(X1,Y1). arc

P2(X2,Y2) intersection point of the line O1P1 with the circle

C2 (calculation of the circle line intersection point coordinates can be found in Appendix 4). By knowing the coordinates of two points P1 and P2, calculate the
distance

d = (P1,P2).
To define the maximum switch arc radius r, use the following system of equations:

(Xo X1)/(X2 X1) = r/d (Yo Y1)/(Y2 Y1) = r/d

(2.3.1-2)

(2.3.1-3)

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-32

(Xo Xc2)2 + (Yo Yc2)2 = (R2 r)2


From (4.1-2)

(2.3.1-4)

Xod X1d = r(X2 X1)


and for

Xo
(2.3.1-5)

Xo = r[(X2 X1)/d] + X1 = rC1 + X1


From (4.1-3)

Yod Y1d = r(Y2 Y1)


and for

Yo
(2.3.1-6)

Yo = r[(Y2 Y1)/d] + Y1 = rC2 + Y1


Substituting (4.5) and (4.6) into (4.4)

[rC1 + X1 Xc2]2 + [rC2 + Y1 Yc2]2 = (R2 r)2


or

(2.3.1-7)

[rC1 + C3]2 + [rC2 + C4]2 = (R2 r)2 (C1)2r2 + (2C1C3)r + (C3)2 + (C2)2r2 + (2C2C4)r + (C4)2 = (R2)2 2R2r + r2

(2.3.1-8)

(2.3.1-9)

Figure 2-29

r2[(C1)2 +(C2)2 1] + r[2C1C3+2C2C4 + 2R2] + (C3)2 +(C4)2(R2)2 = 0


(2.3.1-10) But when considering

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-33

(C1)2 + (C2)2 1 = [(X2 X1)/d]2 + [(Y2 Y1)/d]2 1 = d2/d2 1 = 0


(2.3.1-11)
as d2

= 2(P1,P2) = (X2 X1)2 + (Y2 Y1)2.

and if noting

C6 = 2(C1C3 + C2C4 + R2), C7 = (C3)2 + (C4)2 (R2)2


The results for r are as follows:

r = C7/C6
where

(2.3.1-12)

C1 = (X2 X1)/d C2 = (Y2 Y1)/d C3 = X1 Xc2 C4 = Y1 Yc2 C6 = 2(C1C3 + C2C4 + R2) C7 = (C3)2 + (C4)2 (R2)2
The maximum switch radius can be also estimated as a solution of the alternate system (2.3.1-13)

(Xo X1)/(Xo Xc1) = r/(R1 + r) (Yo Y1)/(Yo Yc1) = r/(R1 + r) (Xo Xc2)2 + (Yo Yc2)2 = (R2 r)2
Its advantage is that there is no need to calculate coordinates of the point P2. From (2.3.1-14),

(2.3.1-14) (2.3.1-15) (2.3.1-16)

Xo = [r(X1 Xc1) + X1R1]/R1 = [r(X1 Xc1)/R1] + X1 = rC1 + X1 (2.3.1-17)


where

C1 = (X1 Xc1)/R1

and from (2.3.1-15)

Yo = [r(Y1 Yc1) + Y1R1]/R1 = [r(Y1 Yc1)/R1] + Y1 = rC1 + Y1 (2.3.1-18)


where C2 =

(Y1 Yc1)/R1
(2.3.1-19)

By substituting (2.3.1-17) and (2.3.1-18) with (2.3.1-16), the results are

[rC1 + X1 Xc2]2 + [rC1 + Y1 Yc2]2 = (R2 r)2


or

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-34

(rC1 + C3)2 + (rC2 + C4)2 = (R2 r)2


where

(2.3.1-20)

C3 = X1 Xc2 and C4 = Y1 Yc2


From (2.3.1-20),

r2C12 + 2C1C3r + C32 + r2C22 + 2C2C4r + C42 = R22 2R2r + r2


(2.3.1-22) but

(2.3.1-21)

r2(C12 + C22 1) + r(2C1C3 + 2C2C4 + 2R2) + (C32 + C42 R22) = 0

C12 + C22 = [(Xc1 X1)2 + (Yc1 Y1)2]/R12 = 1


so we get

(2.3.1-23)

rC6 + C7 = 0 r = C7/C6
where

(2.3.1-24) (2.3.1-25)

C1 = (X1 Xc1)/R1 C2 = (Y1 Yc1)/R1 C3 = X1 Xc2 C4 = Y1 Yc2 C6 = 2C1C3 + 2C2C4 + 2R2 C 7 = C 32 + C 4 2 R 2 2


Example 2-30 (2.3.1-26)

(Motion Mathematic Lib Samples\Circle to Circle\ Section 2_3\ CircleCircle_Ex_2_30 www.elmomc.com)


Circle C1 (Figure 2-30) is defined by its radius R1 = 40000, init point P1(-40000, 0), center coordinates (0,0), init 180o angle and a 90o sweep angle. Circle C2 is defined by the radius R2 = 100000, init point (0, -40000) center coordinates (65000, 35000) and -180o sweep angle. Coordinates of point P1: X1 = -40000, Y1 = 0; To get the coordinates of the point P2, use the circle C2 equation: (X + 65000)2 + (Y 35000)2 = 1000002 Know that Y2 = 0, for X2 use the following equation:

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-35

(X2 + 65000)2 + ( 35000)2 = 1000002 that produces X2 = -158675. d = |X2 X1| = 118675;

C1 = -1.0 C2 = 0 C3 = 25000 C4 = -35000 C6 = 150000 C7 = -8150000000

r = 54333.3333

Figure 2-30 Figure 2-31 shows when the line connecting circle C1 init point (X1,Y1) and its center point do not intersect circle C2. To define the maximum value of the switch radius r, use the following system of equations:

(Xo X1)/(Xc1 X1) = r/R1 (Yo Y1)/(Yc1 Y1) = r/R1 (Xo Xc2)2 + (Yo Yc2)2 = (r + R)2

(2.3.1-27) (2.3.1-28) (2.3.1-29)

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-36

From (2.3.1-27)

Figure 2-31

XoR1 X1R1 = r(Xc1 X1)


or

(2.3.1-30)

Xo = X1 + r[(Xc1 X1)/R1] = X1 + rC1


where

(2.3.1-31) (2.3.1-32)

C1 = (Xc1 X1)/R1

From (2.3.1-28)

YoR1 Y1R1 = r(Yc1 Y1)


or

(2.3.1-33)

Yo = Y1 + r[(Yc1 Y1)/R1] = Y1 + rC2


where

(2.3.1-34) (2.3.1-35)

C2 = [(Yc1 Y1)/R1

Substituting Xo and Yo into (2.3.1-29)

[X1 + rC1 Xc2]2 + [Y1 + rC2 Yc2]2 = (r + R2)2

(2.3.1-36)

If we note that X1 Xc2 = C3 and Y1 Yc2 = C4, then (2.3.1-37) can be written in a format

[rC1 + C3]2 + [rC2 + C4]2 = (r + R2)2


or

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-37

r2 C12 + (2C1C3)r + C32 + r2C22 + (2C2C4)r + C42 = r2 + (2R2)r + R22


(2.3.1-37) or

r2(C12 + C22 1) + 2[C1C3 + C2C4 R2]r + (C32 + C42 R22) = 0


(2.3.1-38) But

C12 + C22 = [(Xc1 X1)2 + (Yc1 Y1)2]/R12 = 1


So (4.1-38) transforms into

(2.3.1-39)

rC6 + C7 = 0
where

(2.3.1-40)

C6 = 2[C1C3 + C2C4 R2]


and

(2.3.1-41)

C 7 = C 3 2 + C 4 2 R2 2
So the an upper limit estimation for r is:

(2.3.1-42)

r = C7/ C6

(2.3.1-29)

Another case when the line O1O2 does not intersect circle arc C2 is shown in the Figure 232. Draw a line O2P2 from the center of the circle C2 to its end point. If noticing that d = (O2P2), the switch arc radius r is:

(X2 Xo)/(X2 Xc2) = r/d (Y2 Yo)/(Y2 Yc2) = r/d (Xo Xc1)2 + (Yo Yc1)2 = (R1 + r)2
From (2.3.1-30)

(2.3.1-30) (2.3.1-31) (2.3.1-32)

X2d Xod = r(X2 Xc2) Xo = X2 r(X2 Xc2)/d = X2 + r(Xc2 X2)/d = X2 + C1r


From (2.3.1-31) (2.3.1-33)

Y2d Yod = r(Y2 Yc2) Yo = Y2 r(Y2 Yc2)/d = Y2 + r(Yc2 Y2)/d = Y2 + C2r


(2.3.1-34)

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-38

Substituting into (4.1-32)

(X2 + C1r Xc1)2 + (Y2 + C2r Yc1)2 = (R1 + r)2 (C1r + C3)2 + (C2r + C4)2 = (R1 + r)2
(2.3.1-37) But

(2.3.1-35) (2.3.1-36)

r2[(C1)2 +(C2)2 1] + r[2C1C3+2C2C4 2R1] + [(C3)2 + (C4)2 (R1)2] = 0

(C1)2 +(C2)2 1 = (Xc2 X2)2/d2 + (Xc2 X2)2/d2 1 = d2/d2 1 = 0


So (2.3.1-38)

r = C7/C6
where

(2.3.1-39)

C1 = (Xc2 X2)/d C2 = (Yc2 Y2)/d C3 = X2 Xc1 C4 = Y2 Yc1 C6 = 2(C1C3 + C2C4 R1) C7 = (C3)2 + (C4)2 (R1)2
(2.3.1-40)

Figure 2-32

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-39

2.3.2 Each circle intersects the internal area of the second


Figure 2-33 shows another case of two circles intersecting: circle C1 comes from inside of the circle C2 and circle C2 continues inside the circle C1. Noting that

d = (O1, O2), the switch radius must obey the following necessary condition: R [d (d R1) (d R2)]/2 = (R1 + R2 d)/2
Condition (2.3.2-1) is sufficient in case line O1O2 intersects circle arc (2.3.2-1)

C1 and circle arc

C2.

Figure 2-33 Line O1O2 does not intersect at least one of two circle segments (Figure 2-35)

(Xc1, Yc1) (Xc2, Yc2) connecting two circle centers does not intersect circle arc C2. Draw a line O2P1 between the circle C2 center and its end point P1(X1,Y1). The center (Xo, Yo) of the switch arc with a maximum radius must belong to this line. To define maximum radius r, use the same system as in the
previous cases.

Another case when line O1O2 does not intersect at least one of the two circle segments. In Figure 2-35 line

(X1 Xo)/(X1 Xc2) = r/R2 (Y1 Yo)/(Y1 Yc2) = r/R2 (Xo Xc1)2 + (Yo Yc1)2 = (R1 r)2

(2.3.2-2) (2.3.2-3) (2.3.2-4)

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-40

This system is similar to (2.3.2-2) where

(2.3.2-4) and comes to the same solution r = C7/C6

C1 = (X1 Xc2)/R2 C2 = (Y1 Yc2)/R2 C3 = Xc1 X1


(2.3.2-5)

C4 = Yc1 Y1 C6 = 2(C1C3 + C2C4 + R1) C7 = (C3)2 + (C4)2 (R1)2

Figure 2-35 Example 2-36

(Motion Mathematic Lib Samples\Circle to Circle\ Section 2_3\ CircleLine_Ex_2_36 www.elmomc.com)


Consider an example (Figure 2-36) of a maximum switch radius calculation for the case similar to the one drawn in Figure 2-35 with the following parameters: Coordinates of the first arc center are Xc1 = 0, center are Xc1 = 100000, Circle arc C1 init angle

Yc1 = 0. Coordinates of the second arc

Yc1 = 100000.

1 = 0o, sweep angle 1 = 90o, radius R1 = 100000. Circle arc C2 init angle 2 = 180o, sweep angle 2 = 30o, radius R2 = 100000.

Calculations due to (4.2-5):

X1 = 100000 100000*cos(pi/6) = 13397.459622 Y1 = 100000 100000*sin(pi/6) = 50000

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-41

C1 = (X1 Xc2)/R2 = -0.866025 C2 = (Y1 Yc2)/R2 = -0.5 C3 = Xc1 X1 = -13397.459622 C4 = Yc1 Y1 = -50000 C6 = 2(C1C3 + C2C4 + R1) = 273205.080767 C7 = (C3)2 + (C4)2 (R1)2 = 7320508075.6888

r = -C7/C6 = 26794.919242
The switch radius in use must be less this value.

Figure 2-36 Line O1O2 does not intersect at least one of the circle segments (Figure 2-37) Another case when the line O1O2 does not intersect at least one of the circle segments. In

Yc1) (Xc2, Yc2) connecting two circle centers does not intersect circle arc C1. Draw a line O1P1 between the circle C1 center and its start point P1(X1,Y1). Center (Xo, Yo) of the switch arc with maximum radius must belong to this line. To define maximum radius r, use the same system as in previous cases: (X1 Xo)/(X1 Xc1) = r/R1 (Y1 Yo)/(Y1 Yc1) = r/R1 (Xo Xc2)2 + (Yo Yc2)2 = (R2 r)2
(2.3.2-6) (2.3.2-7) (2.3.2-8)

Figure 2-37 line (Xc1,

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-42

This system is similar to (4.2)

(4.4) and comes to the same solution r = C7/C6 where

Figure 2-37

C1 = (X1 Xc2)/R1 C2 = (Y1 Yc2)/R1 C3 = Xc2 X1


(2.3.2--9)

C4 = Yc2 Y1 C6 = 2(C1C3 + C2C4 + R2) C7 = (C3)2 + (C4)2 (R2)2


2.3.3 No circle intersects the internal area of the other

In case the circle C2 goes outside circle C1 and circle one circle is less than 90 .
o

C1 doesnt come from the internal area

of the circle C2, restrictions on the switch radius are still possible if a sweep angle of at least

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-43

Consider the case that the sweep angle of the first circle is 1 the second circle is 2

< 90 and the sweep angle of

< 90. Draw a line L1 defined by two points: circle C1 center (Xc1,

Yc1) and circle C1 start point (X1,Y1) and line L2 defined by circle C2 center point (Xc2, Yc2) and circle C2 end point. If line L1 does not intersect circle arc C2 and line L2 does not
intersect the circle arc C1, then the intersection point of two lines is point (X3, 4.3. Note: l1 the length of the line L1 : l1 line L2: l2 If l1

Y3) Figure

= ((X1,Y1),(X3,Y3)) and l2 the length of the

= ((X2,Y2),(X3,Y3)).

>l2 for the maximum switch radius r calculation, use the following system:

(Xo X1)/(X1 Xc1) = r/R1


(2.3.3-1)

(Yo Y1)/(Y1 Yc1) = r/R1


(2.3.3-2)

(Xo Xc2)2 + (Yo Yc2)2 = (R2 + r)


(2.3.3-3) Equations (2.3.3-1)-(2.3.3-2) can be written in the following format

Xo = X1 + r(X1 Xc1)/R1 = X1 + rC1 , C1 = (X1 Xc1)/R1


(2.3.3-4)

Yo = Y1 + r(Y1 Yc1)/R1 = Y1 + rC2 , C2 = (Y1 Yc1)/R1


(2.3.3-5) Substituting into (2.3.3-3), the results are:

(X1 + rC1 Xc2 )2 + (Y1 + rC2 Yc2)2 = (rC1 + C3)2 + (rC2 + C4)2 = (R2 + r)2
(2.3.3-6) where C3

= X1 Xc2 C4 = Y1 Yc2.

Simplifying (2.3.3-6), the results are:

r2[1 (C1)2 (C2)2] + r[2R2 2C1C3 2C2C4] +[(R2)2 (C3)2 (C4)2] =

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-44

r2C5 + rC6 + C7 = 0
But

(2.3.3-7)

1 (C1)2 (C2)2 = 1 (X1 Xc1)2/(R1)2 (Y1 Yc1)2/(R1)2 = 0


So for r the results are:

(2.3.3-8)

r = C7/C6
Similarly in the case when l2

(2.3.3-9)

> l1, use the following system:


(2.3.3-10) (2.3.3-11) (2.3.3-12)

(Xo X2)/(X2 Xc2) = r/R2 (Yo Y2)/(Y2 Yc2) = r/R2 (Xo Xc1)2 + (Yo Yc1)2 = (R1 + r)
Equations (2.3.3-1) - (2.3.3-2) can be written in the following format

Xo = X2 + r(X2 Xc2)/R2 = X2 + rC1 , C1 = (X2 Xc2)/R2 Yo = Y2 + r(Y2 Yc2)/R2 = Y2 + rC2 , C2 = (Y2 Yc2)/R2
And substituting into (2.3.3-3), the results are:

(2.3.3-13) (2.3.3-14)

(X2 + rC1 Xc1 )2 + (Y2 + rC2 Yc1)2 = (rC1 + C3)2 + (rC2 + C4)2 = (R2 + r)2
(2.3.3-15) where C3

= X2 Xc1 C4 = Y2 Yc1.

Simplifying (2.3.3-15), the results are:

r2[1 (C1)2 (C2)2] + r[2R2 2C1C3 2C2C4] +[(R2)2 (C3)2 (C4)2] = r2C5 + rC6 + C7 = 0
But (2.3.3-16)

1 (C1)2 (C2)2 = 1 (X2 Xc2)2/(R2)2 (Y2 Yc2)2/(R2)2 = 0

(2.3.3-17)

Motion Library Tutorial


MAN-MLT (Ver 2.0)

Switch Radius Calculation

2-45

So for r, the results are:

r = C7/C6
If L1 intersects circle arc

(2.3.3-18)

C2, use point (X1,Y1).

If L2 intersects circle arc

C1, use point

(X1,Y1).

Figure 2-38

Maestro Software Manual


MAN-MLT(Ver. 2.0)

A-1

Appendix A: Projection of a point on a line defined by the end points


The line L is defined by its end points P1(X1,Y1) and P2(X2,Y2). Drop a perpendicular from some point (Xo,Yo) on the line L and find coordinates (Xpr, point and the length of the perpendicular h.

Ypr) of the projection

a) X1 = X2 Xpr = X1 Ypr = Yo h = |Xo X1| b) Y2 = Y1 Xpr = Xo Ypr = Y1 h= |Yo Y1| c) X1 X2 and Y2 Y1


Line equation

(X X1)/(X2 X1) = (Y Y1)/(Y2 Y1)


That can be transformed into

(a1.1)

(Y Y1)X = (X X1)Y
or

Y = (Y1dX + XY X1Y)/X = Y1 + k(X X1)


Where

(a1.2)

k = Y/X = (Y2 Y1)/(X2 X1)


(a1.3)

An equation of the perpendicular dropped from (Xo,Yo) on L can be written in a form

(Y - Yo) = (-1/k)(X Xo) = q(X Xo)


or

Y = Yo + q(X Xo)
Substituting (a1.4) into (a1.2) the results are

(a1.4)

Yo + q(X Xo) = Y1 + k(X X1)


That produces for X coordinate

(a1.5)

X = (Yo Y1 + kX1 qXo)/(k q)

(a1.6)

Maestro Software Manual


MAN-MLT(Ver. 2.0)

Setting up the Demo Case

A-2

Y is from (a1.4). Coordinates (X,Y) of the intersection point line L and perpendicular
are coordinates of projection point (Xp,Yp). Having got a projection point (Xp,Yp) we can calculate the perpendicular length h as Euclidian distance

h = [(Xp Xo)2 + (Yp Yo)2]1/2

(a1.7)

Maestro Software Manual


MAN-MLT (Ver. 2.0)

B-1

Appendix B: Coefficients of the line standard equation for the line defined by the end points
If the line L is defined by its end points (X1,Y1) and (X2,Y2) we can calculate the coefficients A,B,C for the three possible cases:

a) X1 X2 and Y1 Y2 .
The line equation can be written in a form

(X X1)/(X2 X1) = (Y Y1)/(Y2 Y1)


or

(a2.1)

(X X1)/ X = (Y Y1)/Y
that can be transformed into

(a2.2)

XY X1Y YX + Y1X = XY YX +(Y1X - X1Y) = 0


From (a2.3) we get

(a2.3)

A = Y, B = X, C = Y1X X1Y b) X1 X2 and Y1 = Y2 .


The line equation is

(a2.4)

Y = Y1 or Y Y1 = 0
So for A,

(a2.5)

B and C we have
(a2.6)

A = 0, B = 1, C = Y1 c) X1 = X2 and Y1 Y2 . The line equation is X = X1 or X X1 = 0


So for

(a2.7)

A, B, C we have
(a2.8)

A = 1, B = 0, C = X1

Maestro Software Manual


MAN-MLT (Ver. 2.0)

C-1

Appendix C: Intersection point of two lines defined by the end points


Line L1 is defined by its end points P1(X1,Y1) and P2(X2,Y2). Line L2 is defined by end points P3(X3,Y3) and P4(X4,Y4). Note: X1 =

X2 X1, Y1 = Y2 Y1, X2 = X4 X3, Y2 = Y4 Y3, k1 = Y1/X1, q1 = X1/Y1, k2 = Y2/X2, q2 = X2/Y2

Calculation of the two lines intersection point (X,Y) depends on the each line position relative to coordinate axes. The following cases are possible.

a) X1 X2, Y1 Y2, X3 X4, Y3 Y4


To define intersection point, use

(X X1)Y1 = (Y Y1)X1 (X X3)Y2 = (Y Y3)X2


or

(a3.1) (a3.2)

XY1 X1Y1 = YX1 Y1X1 XY2 X3Y2 = YX2 Y3X2


From (a3.3) the results are

(a3.3) (a3.4)

X = X1 + Yq1 Y1q1
and substituting into (a3.4) the results are

(a3.5)

(X1 + Yq1 Y1q1)Y2 X3Y2 = YX2 Y3X2


that produces

(a3.6)

Y = [(q1Y1 + X3 X1)Y2 Y3X2]/(q1Y2 X2)


and X can be calculated by (a3.5).

(a3.7)

b) X1 X2, Y1 Y2, X3 = X4, Y3 Y4 To find an intersection point use

(X X1)/X1 = (Y Y1)/Y1
(a3.8)

X = X3
(a3.9)

Maestro Software Manual


MAN-MLT (Ver. 2.0)

Performance Considerations

C-2

or

(X3 X1)/X1 = (Y Y1)/Y1


(a3.10)
and finally for coordinate Y of intersection point the results are

Y = Y1 + k1(X3 X1)
(a3.11) c) X1 X2, Y1 Y2, X3 X4, Y3 = Y4
To find an intersection point use

(X X1)/X1 = (Y Y1)/Y1 Y = Y3
That produces

(a3.12) (a3.13)

X = X1 + q1(Y3 Y1)
d) X1 = X2, Y1 Y2, X3 X4, Y3 Y4
Two lines equations:

(a3.14)

X = X1 (X X3)/X2 = (Y Y3)/Y2
For coordinate Y of intersection point we have

(a3.15) (a3.16)

(X1 X3)/X2 = (Y Y3)/Y2 => Y = Y3 + k2(X1 X3)


e) X1 X2, Y1 = Y2, X3 X4, Y3 Y4
Two lines equations:

(a3.17)

Y = Y1 (X X3)/X2 = (Y Y3)/Y2
For coordinate X of intersection point the results are

(X X3)/X2 = (Y1 Y3)/Y2 => X = X3 + q2(Y1 Y3)


f) X1 = X2, Y1 Y2, X3 X4, Y3 =Y4

(a3.18)

X = X1, Y = Y3
g) X1 X2, Y1 = Y2, X3 = X4, Y3 Y4

X = X3, Y = Y1

Maestro Software Manual


MAN-MLT (Ver. 2.0)

D-1

Appendix D: Circle line intersection points


The line is defined by its end points (X1,Y1) and (X2,Y2). The circle is defined by its radius R and center coordinates (Xo, Yo). Consider the general case X1 X2 and Y1 Y2. In this case, calculate the intersection points using

(X X1)/(X2 X1) = (Y Y1)/(Y2 Y1) (a4.1) (X Xo)2 + (Y Yo)2 = R2 (a4.2) Note that k = (Y2 Y1)/(X2 X1) and C1 = Y1 kX1 equation (a4.1) can be written in
the following form

Y = kX + (Y1 kX1) = kX+ C1


Substituting (a4.3) into (a4.2) results in

(a4.3) (a4.4) (a4.5)

(X Xo)2 + (kX + C1 Yo)2 R2 = 0


Simplifying (a4.4) results in the following equation

C3X2 + C4X + C5 = 0
where

C2 = C1 Yo, C3 = 1 + k2, C4 = 2*(Xo + kC2), C5 = (Xo)2 + (C2)2 R2


Note that d

= (C4)2 4C3C5 and for intersection point X coordinates the results are X1 = (C4 + d1/2)/(2C3) (a4.6) 1/2)/(2C ) X2 = (C4 d (a4.7) 3 Respective Y coordinates can be calculated by (a4.3) as Y1 = kX1 + C1 (a4.8) Y2 = kX2 + C1 (a4.9)
Consider the case

X1 = X2. In this case line equation

X = X1
and substituting into (a4.2) the results are

(Y Yo)2 = R2 (X1 Xo)2


and for Y the results are

(a4.10) (a4.11) (a4.12)

Y 1= Yo + [R2 (X1 Xo)2]1/2 Y 2= Yo [R2 (X1 Xo)2]1/2