1. Kinematics with MPiec Controllers
Hosted by:
Doug Meyer
Sr. Project Engineer
Yaskawa America Inc
Name:
Company:
Address:
Phone:
PP.MPIEC-01 | Rev | Date: 02/20/2012 | ©2012 Yaskawa America, Inc. All rights reserved.

2. Welcome to Yaskawa America’s
Welcome to Yaskawa America’s
Training Café Express
To make this Café enjoyable for all, please follow these tips.
Please do not put us on hold. Others will hear the hold music.
Do not use a speaker phone. Background noise can be heard.
We welcome comments and questions. You can type questions into the “Chat” window. Please send to ‘All Panelists’
Questions not answered during the Café can be ed to or can be entered into the survey sent to you at the end of the class.
Name:
Company:
Address:
Phone:
PP.YEA-M.01 | Rev | Date: 03/31/2011 | ©2010 Yaskawa America, Inc. All rights reserved.

3. Challenge #1 2-axis ‘Delta-2’ Planar Robot
Pick and place action
cycles per min
Constant pickup location
Create a product stack
Move along a Path to avoid abrupt corners and improve cycle time
Keep path referenced in X-Z cartesian space x z

4. Delta-2 Approach Benefits over traditional Gantry
Higher speeds at end-effector
Fixed motor locations
Stability and rigidity
Multiple mechanical arrangements and permutations are possible x z

5. Solution Sigma-5 motors with Absolute Feeedback MP2300Siec Controller
No homing required on power-up
MP2300Siec Controller
1-16 axes networked servo control
Up to 6 Virtual axes
MotionWorks IEC Professional
IEC
User-defined Function Blocks
User-defined DataTypes

6. Application Support Tools
NEW
Kinematic_Toolbox_v200
Delta_2_1_Control
FourBar_1_Control
Gantry_Toolbox_v201
XY_MoveAbsolute
PathGenerator
MovePath
Math_Toolbox_v201
ATAN2
ArcTan2 function avoids discontinuities of regular ArcTan function at vertical asymptotes
YMotion Firmware Library
Y_DirectControl
Allows continuous streaming of position, velocity or torque targets to a motion axis

7. Implementation Method – A 5-Step Process
1) Define Virtual Axes in user coordinate system (X,Z) 2) Use Forward Kinematic equations to establish initial position of the virtual axes 3) Enable all axes 4) Create motion on the virtual axes in (X,Z) cartesian space 5) Use Inverse Kinematic equations to transfer the motion to the real axes in (Θ1,Θ2) mechanism space

8. How to Implement Add Kinematic Toolbox as User Library
Available on Yaskawa Website
Free download
Currently contains 2 kinematic mechanisms
More to follow

9. How to Implement One Function Block Inserted into a POU!
Enables the Axis
Clears Alarms
Sets Zero Position for Mechanism
All Kinematics inside
Just supply information about the mechanism and the application

10. How to Implement Two parts of the implementation Example Code
Run the Kinematic Function Block
Create motion on the virtuals in Cartesian space
Example Code
Delta2_1_EC_v200.zwt
Website Document Number EC.MWIEC.31

11. Inside the Function Block

12. Model Givens: d L1=L2 L3=L4 L4+L5 F L5 C L4 L3 Θ4 Θ3 D B P z’ L1 L2 Qx’ z’ L2 L3 L4 L1 A B C D E Θ2 Θ1 Θ4 F P Q d L5 Θ3
Model
Givens: d
L1=L2
L3=L4
L4+L5
Note:
Shown Inverted from normal usage so as to align with linkage coordinate system
Motors located at pts A and E
Coordinate system origin at Pt A

13. Method Prior to enabling servos Forward Kinematics
Establish (x’,z’) position of linkage based on (Θ1,Θ2) absolute motor positions
Coordinate system transformation
Translate (x’,z’) positions of the linkage into (X,Z) positions of end effector
Set positions of Virtual Axes in (X,Z) space

14. Forward Kinematics Custom Function Block
Uses abs position of motors at Pts A and E (Θ1 and Θ2 are known)
Geometry and Trigonometry
Θ4 can only be calculated after PtC is known
Used to establish position of Pt F prior to enabling the real axes.
A = (0,0)
E = (d,0)
D = (d + L1CosΘ1, L1SinΘ1)
B = (L2CosΘ2, L2SinΘ2)
C = Intersection of Circles centered at B and D
F = (d + L1CosΘ1 + L45CosΘ4, L1SinΘ1 + L45SinΘ4) = (Xf, Zf)

15. Coordinate System Fwd Translation to X,Z
(0,0)
X = Xoffset - X’
Z = Zoffset - Z’ - Zef
Z Offset x z F
(x’,z’)  (x,z+zef)
Zef End-effector offset
(x,z)
(0,0)
X offset

16. Creating Movement on Virtual Axes (X,Z)
Application code creates moves in X,Z space using XY_MoveAbsolute, PathGenerator and Move_Path function blocks found in the Gantry_Toolbox. See Example Code Delta2_1_EC_v200.zwt.

17. Method to Define Paths
Pre-Calculate path segments using a customized Path Segment Calculator FB in a lower speed task
Define Segments in absolute X,Z space
Generate path segment structure
Ex. Path ‘Drop3’ moves from pickup point to drop off point for layer 3.
Seg2
Seg3
Seg4
Seg5
(X1,Z1)
(X2,Z2)
(X3,Z3)
(X4,Z4)
(X6,Z6)
(X5,Z5)
Seg1 x z

18. Segment calculation Straight Line definition
Straight lines can be defined by 2 absolute points and by setting Resolution equal to zero.
Starting point is assumed to be current position
Specify total number of segments when finished with segment definition
Drop3Segs.Data[2].SegmentType := TB_Pattern#StraightLine;
Drop3Segs.Data[2].Xcoord := X3;
Drop3Segs.Data[2].Ycoord := Z3;
Drop3Segs.Data[2].Resolution := REAL#0.0;
Drop3Segs.LastSegment := INT#4;

19. Segment calculation Arc definition
Arcs can be defined given the start and end points, absolute starting angle and traversed angle
If the angles are unknown, use Calculate_Angles function given the points, radius and direction (ArcDefinitionMode = INT#1) OR center point and direction of the arc (INT#0).
Drop3Segs.Data[1].SegmentType := TB_Pattern#Arc;
Drop3Segs.Data[1].Radius := R1;
Drop3Segs.Data[1].Resolution := REAL#0.1;
Calculate_Angles_1(X1:=X1,X2:=X2,Y1:=Z1,Y2:=Z2,Radius:=R1,Execute:= TRUE,
ArcDefinitionMode:=INT#1,Direction:=BOOL#0); (* 0=CW, 1=CCW *)
Drop3Segs.Data[1].StartAngle := Calculate_Angles_1.StartAngle;
Drop3Segs.Data[1].TraversedAngle := Calculate_Angles_1.TraversedAngle;

20. Path Segment Calculator
A customized segment calculator block is used to create the path segments from user-provided datapoints.
Provide array of data points
Creates a segment structure to be fed to the PathGenerator

21. PathGenerator
PathGenerator function block turns segment data into a predefined path on the prescribed axes
Provide segment structure
Returns Path structure and PathID number to use with Move_Path

22. Move the Virtual Axes in X,Z space
Pre-Defined Paths can be executed with Move_Path

23. Move the Virtual Axes in X,Z space
Normal pt-to-pt moves can be done with MC_MoveAbsolute or XY_MoveAbsolute on the virtuals defined in a Gantry structure

24. Method During Operation the process is inverted
Create motion on Virtual Axes in (X,Z) Space
XY_MoveAbsolute for pt to pt moves
Path Generator for paths made of linear and circular segments
Coordinate system transformation
Translate (X,Z) actual positions of end effector into (x’,z’) commanded positions of the robot linkage
Inverse Kinematics
Transform (x’,z’) commanded positions into (Θ1,Θ2) commands
Y_DirectControl Function Block streams position targets into real Θ1, and Θ2 axes.

25. Inverse Kinematics, 3 Steps
1) Translate Commanded Position of virtual axes in (X,Z) into Commanded Position of PtF in (x’,z’)

26. Coordinate System Translation to x’,z’
(0,0)
x’ = Xoffset - X
z’ = Zoffset - Z - Zef
Z Offset x z F
(x’,z’)  (x,z+zef)
Zef End-effector offset
(x,z)
(0,0)
X offset

27. Inverse Kinematics, 3 Steps
2) Feed Commanded position of PtF into an Inverse Kinematics function block

28. Inverse Kinematics Custom Function Block
Uses desired (Xf,Zf) position of Virtual Axes
Geometry and Trigonometry transformation
Used to calculate commanded position of real axes.
Θ1 = P - Q
Θ4 = ATAN2[(Zf – L1SinΘ1)/(Xf – L1CosΘ1 – d)]
Xc = Xf – L5CosΘ4
Zc = Zf – L5SinΘ4
Θ2= ATAN2(Zc/Xc) + Cos-1[(L22+Xc2+Zc2-L32) / (2L2(Xc2+Zc2)1/2)]

29. Inverse Kinematics, 3 Steps
3) Feed the resulting commanded position values for Theta1 and Theta2 into a Y_DirectControl block for each axis.
Y_DirectControl is found in the Y_Motion Firmware Library

30. Stream Command to Real Axes
Y_DirectControl Block
From YMotion firmware library
Sub-Interpolation Filter smoothes the profile
More sub-interpolation at Servo

31. Set Limits on Range of Motion
X offset x z
(0,0)
Choose Valid XZ Coordinate System for Virtual offsets
All points must be within physical range of motion
In general, operate below the ‘knees’ of the mechanism
Leave margin to avoid full extension
Z Offset
Approximate range of motion

32. Set Limits on Range of MotionC
Prevent Multiple solutions to Inverse Kinematic equations
Choose good starting point and path
Stay away from points that create ‘locked legs’ condition
LenAC approaches L2+L3 (or Θ2= Θ3)
LenEC approaches L1+L4 (or Θ1= Θ4) L4 L3 Θ3 Θ4 B D L2 L1 x’ z’ Θ2 Θ1 A E d

33. Set Limits on Range of Motion
Prevent Linkage overlaps
Set Min and Max value for Θ1
Set Min and Max value for Θ2 F x’ z’ B L5 L3 L2 Θ2 C A Θ1 E d L1 L4 Θ3 D

34. Set Limits on Range of Motion
Prevent Linkage overlaps (linkage compression)
Θ4 – Θ3 approaches 180 deg L5 L4 C Θ4 L3 Θ3 B x’ z’ D Θ2 L1 L2 Θ1 A E d

35. Set Limits on Range of Motion
Enter Givens and Limits in Initialization POU

36. Example Video
To see an example of Yaskawa Kinematic Toolbox in action on a real machine, view the video available on YouTube at This video shows a pick and place arm in action on a Blisterpack Thermoformer machine at PackExpo 2011.

37. Challenge #2 2-axis ‘4-Bar’ Planar Robot
Pick and place action
Concentric DD motors
Independent Z axis
Improve repeatability, throughput and uptime
Target +/- 12 micron repeatability
Faster moves - Double production rate to 4000 substrates per day
Path is referenced in X-Y Cartesian space

38. Method Process is the same Create Forward Kinematic equations
Translate to working coordinate system through offsets
Create motion on virtual axes in XY space
Translate virtual positions into mechanism coordinate space
Perform Inverse Kinematics on mechanism endpoint to create motor commands
Stream motor position commands to real axes using Y_DirectControl block.

39. Model Advantages of 4-bar linkage
Less moving inertia since motor controlling L2 is at origin of L1 instead of end of L1
Greater stability of L2
FB assumes a parallelogram structure 4 M

40. Kinematics

41. Kinematics

42. Example Code FourBar_1_EC_v200.zwt
Available at Yaskawa .com Document Number EC.MWIEC.32

43. Example Video
To see an example of Yaskawa Kinematic Toolbox in action for the Four-Bar Linkage, view the video available on YouTube at This video shows an arm in action moving around the motion envelope at 2g acceleration.

44. Questions and Answers

45. Thank You Name: Company: Address: Phone:
PP.MPIEC.01 | Rev | Date: 03/31/2011 | ©2010 Yaskawa America, Inc. All rights reserved.