The Visualization and Optimization of the Dynamic Workspace of a Delta Robot

A PLAN B PROJECT REPORT

SUBMITTED TO THE FACULTY OF THE GRADUATE SCHOOL

OF UNIVERSITY OF MINNESOTA

BY

Tiance Xia

IN PARTIAL FULFILLMENT OF THE REQUIREMENTS

FOR THE DEGREE OF

MASTER OF SCIENCE IN MECHANICAL ENGINEERING

Timothy M. Kowalewski, PhD

September, 2018

The visualization and optimization of the dynamic workspace of a Delta robot

Tiance Xia

Department of Mechanical Engineering

University of Minnesota

I. Introduction

The parallel manipulator is a mechanical system where the base and the end effector are

connected by more than one series of mechanical links. Often each series of mechanical links has

identical structure. Compared with serial manipulators, parallel manipulators achieve higher

velocities and precision of motion and support more massive payloads, at the cost of reduced

workspace. The common examples included the Stewart platform used in flight simulators (Fig.1)

and the Delta robot used in pick/pack applications. (Fig.2) For the Delta robot, its kinematics

solution and the distribution of the singularities had been studied extensively. [1][2] However, for

the dynamics analysis, although the closed form solution of the motor torque had been given, either

via the Newton-Euler method [3][4][5] or the d’Alembert’s principle [6][7][8], the structure of the

robot dynamic workspace remains understudied. The dynamic workspace indicates the region

where the end effector can move at the designated velocity and acceleration, given the robot size,

mass and the motor data. The establishment of this workspace would facilitate the end users to

choose the appropriate robot model for their task.

The overall goal of this research is to provide an improved, easy-to-use tool to assist

hobbyists, engineering students, and professional engineers in the computational design of high

performance Delta robots for their custom applications. The specific aim of this research is to

derive the dynamic solutions of the Delta robot in detail, and elucidate both the reachable

workspace and the dynamic workspace of the robot as a function of robot link length and motor

performance specifications as dictated by the readily available motor torque-speed curves.

Fig.1. The CAE 7000XR full-flight simulator. Fig.2. The ABB IRB 360 FlexPicker.

The specific contributions are 1) a re-derivation of the Delta robot dynamics using

mechanical energy conservation principles; 2) visualization of the complicated reachable and

dynamic workspaces and how they change with robot and motor parameters; 3) a software toolkit

in MATLAB with a graphical user interface (GUI) that visualizes complicated reachable and

dynamic workspaces and how they change based on simple motor parameters that a user may enter

to intuitively explore whether a specific choice of motors and link lengths will achieve the desired

performance characteristics; and 4) an example of such design analyses for a specific handheld 3D

bio-printing robot designed for medical applications in the Medical Robotics and Devices Lab.

II. Theory

The Delta robot is composed of three elements: the base, the moving platform and three

groups of connecting arms. (Fig.3a) Three active joints (motors) are placed on the base. The

connecting points of the moving platform are at the vertices of the triangle. For simplicity, the

parallelogram lower arm in the actual robot was represented by a single solid link. (Fig.3b)

Fig.3a. The assembly diagram of the Delta robot. Fig.3b. The Delta robot in Cartesian

coordinate system. The parallelogram

meant the forearm could move in both

XZ plane and YZ plane.

Because the moving platform only had three degrees of freedom for translational motion, the

following parallel conditions were always satisfied:

𝑂𝐴1 ∥ 𝑃𝐶1

𝑂𝐴2 ∥ 𝑃𝐶2

𝑂𝐴3 ∥ 𝑃𝐶3

The geometric size of the robot was given by:

𝑂𝐴1 = 𝑂𝐴2 = 𝑂𝐴3 = 𝑅

𝑃𝐶1 = 𝑃𝐶2 = 𝑃𝐶3 = 𝑟

𝐴1𝐵1 = 𝐴2𝐵2 = 𝐴3𝐵3 = 𝐿

𝐵1𝐶1 = 𝐵2𝐶2 = 𝐵3𝐶3 = 𝑙

Points M and N represented the midpoint of the link, which were used for inertial analysis. In

addition, in this paper, the upper links were not allowed to go above the XY plane. Thus the arm

rotational angle 𝜃 was always negative.

To make the structure clear, the establishment of the dynamic workspace had been divided

to three sections: the dynamic solution of the motor torque, the manipulator Jacobian, and the

algorithm to search the dynamic workspace.

1. Motor Torques

A. Link Dynamics

From the robot diagram (Fig.3b), the following relationships could be established:

𝐴1 = (𝑅, 0, 0)

𝐵1 = (𝑅 + 𝐿𝑐𝑜𝑠𝜃1, 0, 𝐿𝑠𝑖𝑛𝜃1)

𝑃 = (𝑥𝑝, 𝑦𝑝, 𝑧𝑝)

𝐶1 = (𝑥𝑝 + 𝑟, 𝑦𝑝, 𝑧𝑝)

𝑀1 =𝐴1 + 𝐵12

= (1

2(2𝑅 + 𝐿𝑐𝑜𝑠𝜃1), 0,

1

2𝐿𝑠𝑖𝑛𝜃1)

𝑁1 =𝐵1 + 𝐶12

= (1

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃1 + 𝑥𝑝 + 𝑟),

1

2𝑦𝑝,1

2(𝐿𝑠𝑖𝑛𝜃1 + 𝑧𝑝))

The velocity at point 𝑀1:

𝑑

𝑑𝑡𝑂𝑀1⃗⃗ ⃗⃗ ⃗⃗ ⃗⃗ = (

1

2(−𝐿𝑠𝑖𝑛𝜃1 ∙ 𝜃1̇), 0,

1

2𝐿𝑐𝑜𝑠𝜃1 ∙ 𝜃1̇)

The velocity at point 𝑁1:

𝑑

𝑑𝑡𝑂𝑁1⃗⃗ ⃗⃗ ⃗⃗ ⃗⃗ = (

1

2(−𝐿𝑠𝑖𝑛𝜃1 ∙ 𝜃1̇ + 𝑥�̇�),

1

2𝑦�̇�,1

2(𝐿𝑐𝑜𝑠𝜃1 ∙ 𝜃1̇ + 𝑧�̇�))

The tangential velocity at point 𝐶1:

𝐵1𝐶1⃗⃗ ⃗⃗ ⃗⃗ ⃗⃗ ⃗ = (𝑥𝑝 + 𝑟 − 𝑅 − 𝐿𝑐𝑜𝑠𝜃1, 𝑦𝑝, 𝑧𝑝 − 𝐿𝑠𝑖𝑛𝜃1)

𝑣𝐶1⃗⃗⃗⃗⃗⃗ =𝑑

𝑑𝑡𝐵1𝐶1⃗⃗ ⃗⃗ ⃗⃗ ⃗⃗ ⃗ = (�̇�𝑝 + 𝐿𝑠𝑖𝑛𝜃1 ∙ 𝜃1̇, 𝑦�̇�, 𝑧�̇� − 𝐿𝑐𝑜𝑠𝜃1 ∙ 𝜃1̇)

In 3D space, the angular velocity of one point rotating around an axis was given by:

�⃗⃗� =𝑟 × 𝑣

𝑟2

Therefore, the angular velocity of link 𝐵1𝐶1:

𝑑

𝑑𝑡𝜃𝐵1 = 𝜔𝐵1⃗⃗ ⃗⃗ ⃗⃗ ⃗ =

1

𝑙2(𝐵1𝐶1⃗⃗ ⃗⃗ ⃗⃗ ⃗⃗ ⃗ × 𝑣𝐶1⃗⃗⃗⃗⃗⃗ )

=1

𝑙2(

𝑦𝑝(𝑧�̇� − 𝐿𝑐𝑜𝑠𝜃1 ∙ 𝜃1̇) − (𝑧𝑝 − 𝐿𝑠𝑖𝑛𝜃1)𝑦�̇�,

(𝑧𝑝 − 𝐿𝑠𝑖𝑛𝜃1)(𝑥�̇� + 𝐿𝑠𝑖𝑛𝜃1 ∙ 𝜃1̇) − (𝑥𝑝 + 𝑟 − 𝑅 − 𝐿𝑐𝑜𝑠𝜃1)(𝑧�̇� − 𝐿𝑐𝑜𝑠𝜃1 ∙ 𝜃1̇),

(𝑥𝑝 + 𝑟 − 𝑅 − 𝐿𝑐𝑜𝑠𝜃1)𝑦�̇� − 𝑦𝑝(𝑥�̇� + 𝐿𝑠𝑖𝑛𝜃1 ∙ 𝜃1̇)

)

Let

𝐾1𝑠 = 𝐿𝑠𝑖𝑛𝜃1 𝐾1𝑐 = 𝐿𝑐𝑜𝑠𝜃1

The differential of the rotational angle of link 𝐵1𝐶1 was given by:

𝑑𝜃𝐵1

=1

𝑙2(

𝑦𝑝(𝑑𝑧𝑝 − 𝐿𝑐𝑜𝑠𝜃1 ∙ 𝑑𝜃1) − (𝑧𝑝 − 𝐿𝑠𝑖𝑛𝜃1)𝑑𝑦𝑝,

(𝑧𝑝 − 𝐿𝑠𝑖𝑛𝜃1)(𝑑𝑥𝑝 + 𝐿𝑠𝑖𝑛𝜃1 ∙ 𝑑𝜃1) − (𝑥𝑝 + 𝑟 − 𝑅 − 𝐿𝑐𝑜𝑠𝜃1)(𝑑𝑧𝑝 − 𝐿𝑐𝑜𝑠𝜃1 ∙ 𝑑𝜃1),

(𝑥𝑝 + 𝑟 − 𝑅 − 𝐿𝑐𝑜𝑠𝜃1)𝑑𝑦𝑝 − 𝑦𝑝(𝑑𝑥𝑝 + 𝐿𝑠𝑖𝑛𝜃1 ∙ 𝑑𝜃1)

)

= (

𝐹1(𝑑𝑧𝑝 − 𝐾1𝑐 ∙ 𝑑𝜃1) − 𝐹2 ∙ 𝑑𝑦𝑝,

𝐹2(𝑑𝑥𝑝 + 𝐾1𝑠 ∙ 𝑑𝜃1) − 𝐹3(𝑑𝑧𝑝 − 𝐾1𝑐 ∙ 𝑑𝜃1),

𝐹3 ∙ 𝑑𝑦𝑝 − 𝐹1(𝑑𝑥𝑝 + 𝐾1𝑠 ∙ 𝑑𝜃1)

)

If the payload underwent an infinitesimal displacement, there would be a corresponded

position change for the link group 𝐴1𝐵1 − 𝐵1𝐶1. The work needed to move the link group from its

original location was represented by:

𝑊𝐿𝐺1 = 𝑊𝐴1𝐵1 +𝑊𝐵1𝐶1

= 𝑊𝐴1𝐵1,𝐺𝑟𝑎𝑣𝑖𝑡𝑦 +𝑊𝐴1𝐵1,𝑅𝑜𝑡𝑎𝑡𝑖𝑜𝑛 +𝑊𝐵1𝐶1,𝐺𝑟𝑎𝑣𝑖𝑡𝑦 +𝑊𝐵1𝐶1,𝑇𝑟𝑎𝑛𝑠𝑙𝑎𝑡𝑖𝑜𝑛 +𝑊𝐵1𝐶1,𝑅𝑜𝑡𝑎𝑡𝑖𝑜𝑛

= (𝑚𝐿𝑔 ∙ 𝛿𝑀1𝑧 + 𝐼𝐿𝜃1̈ ∙ 𝑑𝜃1) + (𝑚𝑙𝑔 ∙ 𝛿𝑁1𝑧 +𝑚𝑙𝑎𝑁1 ∙ 𝛿𝑁1 + 𝐼𝑙𝜔𝐵1̇ ∙ 𝑑𝜃𝐵1)

, where 𝛿𝑀1𝑧 and 𝛿𝑁1𝑧 represented the displacement of M1 and N1 in z-direction, 𝐼𝐿 and 𝐼𝑙

represented the moment of inertia of link 𝐴1𝐵1 and 𝐵1𝐶1 rotating around the axes that went through

𝐴1 and 𝐵1 respectively, 𝑎𝑁1 represented the acceleration of point 𝑁1, and 𝜔𝐵1̇ represented the

angular acceleration of link 𝐵1𝐶1.

𝑎𝑁1 =𝑑

𝑑𝑡(𝑑

𝑑𝑡𝑂𝑁1⃗⃗ ⃗⃗ ⃗⃗ ⃗⃗ ) = (𝑎𝑁1𝑥, 𝑎𝑁1𝑦, 𝑎𝑁1𝑧)

𝜔𝐵1̇ =𝑑

𝑑𝑡(𝜔𝐵1⃗⃗ ⃗⃗ ⃗⃗ ⃗) = (𝜔𝐵1𝑥̇ , 𝜔𝐵1𝑦̇ , 𝜔𝐵1𝑧̇ )

Based on the expressions of 𝑑

𝑑𝑡𝑂𝑀1⃗⃗ ⃗⃗ ⃗⃗ ⃗⃗ and

𝑑

𝑑𝑡𝑂𝑁1⃗⃗ ⃗⃗ ⃗⃗ ⃗⃗ ,

𝛿𝑀1𝑧 =1

2𝐿𝑐𝑜𝑠𝜃1 ∙ 𝑑𝜃1 =

1

2𝐾1𝑐 ∙ 𝑑𝜃1

𝛿𝑁1 = (1

2(−𝐿𝑠𝑖𝑛𝜃1 ∙ 𝑑𝜃1 + 𝑑𝑥𝑝),

1

2𝑑𝑦𝑝,

1

2(𝐿𝑐𝑜𝑠𝜃1 ∙ 𝑑𝜃1 + 𝑑𝑧𝑝))

= (1

2(−𝐾1𝑠 ∙ 𝑑𝜃1 + 𝑑𝑥𝑝),

1

2𝑑𝑦𝑝,

1

2(𝐾1𝑐 ∙ 𝑑𝜃1 + 𝑑𝑧𝑝))

𝛿𝑁1𝑧 =1

2(𝐿𝑐𝑜𝑠𝜃1 ∙ 𝑑𝜃1 + 𝑑𝑧𝑝) =

1

2(𝐾1𝑐 ∙ 𝑑𝜃1 + 𝑑𝑧𝑝)

Therefore, the work equation could be expanded as:

𝑊𝐴1𝐵1 +𝑊𝐵1𝐶1

= 𝑚𝐿𝑔 ∙1

2𝐾1𝑐 ∙ 𝑑𝜃1 + 𝐼𝐿𝜃1̈ ∙ 𝑑𝜃1 +𝑚𝑙𝑔 ∙

1

2(𝐾1𝑐 ∙ 𝑑𝜃1 + 𝑑𝑧𝑝)

+𝑚𝑙 [𝑎𝑁1𝑥 ∙1

2(−𝐾1𝑠 ∙ 𝑑𝜃1 + 𝑑𝑥𝑝) + 𝑎𝑁1𝑦 ∙

1

2𝑑𝑦𝑝 + 𝑎𝑁1𝑧 ∙

1

2(𝐾1𝑐 ∙ 𝑑𝜃1 + 𝑑𝑧𝑝)]

+𝐼𝑙 [𝜔𝐵1𝑥̇ (𝐹1(𝑑𝑧𝑝 − 𝐾1𝑐 ∙ 𝑑𝜃1) − 𝐹2 ∙ 𝑑𝑦𝑝)

+ 𝜔𝐵1𝑦̇ (𝐹2(𝑑𝑥𝑝 + 𝐾1𝑠 ∙ 𝑑𝜃1) − 𝐹3(𝑑𝑧𝑝 − 𝐾1𝑐 ∙ 𝑑𝜃1)) +̇

𝜔𝐵1𝑧̇ (𝐹3 ∙ 𝑑𝑦𝑝

− 𝐹1(𝑑𝑥𝑝 + 𝐾1𝑠 ∙ 𝑑𝜃1))]

= (𝑚𝐿𝑔 ∙1

2𝐾1𝑐 + 𝐼𝐿𝜃1̈ +𝑚𝑙𝑔 ∙

1

2𝐾1𝑐 −𝑚𝑙𝑎𝑁1𝑥 ∙

1

2𝐾1𝑠 +𝑚𝑙𝑎𝑁1𝑧 ∙

1

2𝐾1𝑐 − 𝐼𝑙𝜔𝐵1𝑥̇ 𝐹1𝐾1𝑐

+ 𝐼𝑙𝜔𝐵1𝑦̇ 𝐹2𝐾1𝑠 + 𝐼𝑙𝜔𝐵1𝑦̇ 𝐹3𝐾1𝑐 − 𝐼𝑙𝜔𝐵1𝑧̇ 𝐹1𝐾1𝑠) 𝑑𝜃1

+ (𝑚𝑙𝑎𝑁1𝑥 ∙1

2+ 𝐼𝑙𝜔𝐵1𝑦̇ 𝐹2 − 𝐼𝑙𝜔𝐵1𝑧̇ 𝐹1)𝑑𝑥𝑝

+ (𝑚𝑙𝑎𝑁1𝑦 ∙1

2− 𝐼𝑙𝜔𝐵1𝑥̇ 𝐹2 + 𝐼𝑙𝜔𝐵1𝑧̇ 𝐹3)𝑑𝑦𝑝

+ (𝑚𝑙𝑎𝑁1𝑧 ∙1

2+ 𝐼𝑙𝜔𝐵1𝑥̇ 𝐹1 − 𝐼𝑙𝜔𝐵1𝑦̇ 𝐹3 +𝑚𝑙𝑔 ∙

1

2) 𝑑𝑧𝑝

= 𝑇𝐴𝑑𝜃1 + 𝑇11𝑑𝑥𝑝 + 𝑇12𝑑𝑦𝑝 + 𝑇13𝑑𝑧𝑝

The work equations for link group 𝐴2𝐵2 − 𝐵2𝐶2 and link group 𝐴3𝐵3 − 𝐵3𝐶3 could be

derived via same procedures. (Their positions would also change if the payload moved) To keep

the logic clear, only the final results were quoted here and the detailed expressions could be found

in the Appendix.

𝑊𝐴2𝐵2 +𝑊𝐵2𝐶2 = 𝑇𝐵𝑑𝜃2 + 𝑇21𝑑𝑥𝑝 + 𝑇22𝑑𝑦𝑝 + 𝑇23𝑑𝑧𝑝

𝑊𝐴3𝐵3 +𝑊𝐵3𝐶3 = 𝑇𝐶𝑑𝜃3 + 𝑇31𝑑𝑥𝑝 + 𝑇32𝑑𝑦𝑝 + 𝑇33𝑑𝑧𝑝

B. Payload Dynamics

The work to move the end effector for an infinitesimal displacement 𝛿𝑃 was given by:

𝑊𝑝𝑎𝑦𝑙𝑜𝑎𝑑 = 𝑚𝑃𝑔 ∙ 𝛿𝑃𝑧 +𝑚𝑃𝑎𝑃 ∙ 𝛿𝑃

, where 𝛿𝑃 = (𝑑𝑥𝑝, 𝑑𝑦𝑝, 𝑑𝑧𝑝), 𝛿𝑃𝑧 = 𝑑𝑧𝑝, and 𝑎𝑃 = (𝑎𝑃𝑥, 𝑎𝑃𝑦, 𝑎𝑃𝑧).

Therefore, the payload work could be written as:

𝑊𝑝𝑎𝑦𝑙𝑜𝑎𝑑 = 𝑚𝑃𝑔 ∙ 𝑑𝑧𝑝 +𝑚𝑃𝑎𝑃𝑥𝑑𝑥𝑝 +𝑚𝑃𝑎𝑃𝑦𝑑𝑦𝑝 +𝑚𝑃𝑎𝑃𝑧𝑑𝑧𝑝

C. The Torque Formula

By the energy conservation principle (or the d’Alembert principle), neglecting the friction,

the work done to move the links and the payload equaled the work outputted by the three motors.

Namely,

𝜏1𝑑𝜃1 + 𝜏2𝑑𝜃2 + 𝜏3𝑑𝜃3 +𝑊𝑒𝑥𝑡𝑒𝑟𝑛𝑎𝑙 = 0

, where 𝜏1, 𝜏2, 𝜏3 represented the torque the motor generated, and

𝑊𝑒𝑥𝑡𝑒𝑟𝑛𝑎𝑙 = 𝑊𝐴1𝐵1 +𝑊𝐵1𝐶1 +𝑊𝐴2𝐵2 +𝑊𝐵2𝐶2 +𝑊𝐴3𝐵3 +𝑊𝐵3𝐶3 +𝑊𝑝𝑎𝑦𝑙𝑜𝑎𝑑

(1)

= (𝑇𝐴𝑑𝜃1 + 𝑇11𝑑𝑥𝑝 + 𝑇12𝑑𝑦𝑝 + 𝑇13𝑑𝑧𝑝) + (𝑇𝐵𝑑𝜃2 + 𝑇21𝑑𝑥𝑝 + 𝑇22𝑑𝑦𝑝 + 𝑇23𝑑𝑧𝑝)

+ (𝑇𝐶𝑑𝜃3 + 𝑇31𝑑𝑥𝑝 + 𝑇32𝑑𝑦𝑝 + 𝑇33𝑑𝑧𝑝)

+ (𝑚𝑃𝑔 ∙ 𝑑𝑧𝑝 +𝑚𝑃𝑎𝑃𝑥𝑑𝑥𝑝 +𝑚𝑃𝑎𝑃𝑦𝑑𝑦𝑝 +𝑚𝑃𝑎𝑃𝑧𝑑𝑧𝑝)

= 𝑇𝐴𝑑𝜃1 + 𝑇𝐵𝑑𝜃2 + 𝑇𝐶𝑑𝜃3 + (𝑇11 + 𝑇21 + 𝑇31 +𝑚𝑃𝑎𝑃𝑥)𝑑𝑥𝑝

+ (𝑇12 + 𝑇22 + 𝑇32 +𝑚𝑃𝑎𝑃𝑦)𝑑𝑦𝑝 + (𝑇13 + 𝑇23 + 𝑇33 +𝑚𝑃𝑎𝑃𝑧 +𝑚𝑃𝑔)𝑑𝑧𝑝

= 𝑇𝐴𝑑𝜃1 + 𝑇𝐵𝑑𝜃2 + 𝑇𝐶𝑑𝜃3 +𝑈1𝑑𝑥𝑝 + 𝑈2𝑑𝑦𝑝 + 𝑈3𝑑𝑧𝑝

The manipulator Jacobian, which would be derived in the next section, established the

connection between the end effector spatial velocity and the motor angular velocity. For the delta

robot, the manipulator Jacobian J was a 3-by-3 matrix since there was no rotational motion for the

moving platform. From the expression 𝑿�̇� = 𝑱 ∙ �̇�, the differentials of the end effector translational

displacement could be converted to the differentials of the motor rotational angles.

𝑥�̇� = 𝐽11𝜃1̇ + 𝐽12𝜃2̇ + 𝐽13𝜃3̇ → 𝑑𝑥𝑝 = 𝐽11𝑑𝜃1 + 𝐽12𝑑𝜃2 + 𝐽13𝑑𝜃3

𝑦�̇� = 𝐽21𝜃1̇ + 𝐽22𝜃2̇ + 𝐽23𝜃3̇ → 𝑑𝑦𝑝 = 𝐽21𝑑𝜃1 + 𝐽22𝑑𝜃2 + 𝐽23𝑑𝜃3

𝑧�̇� = 𝐽31𝜃1̇ + 𝐽32𝜃2̇ + 𝐽33𝜃3̇ → 𝑑𝑧𝑝 = 𝐽31𝑑𝜃1 + 𝐽32𝑑𝜃2 + 𝐽33𝑑𝜃3

Therefore, the last half of equation (2) could be written as:

𝑈1𝑑𝑥𝑝 + 𝑈2𝑑𝑦𝑝 + 𝑈3𝑑𝑧𝑝

= (𝑈1𝐽11 + 𝑈2𝐽21 + 𝑈3𝐽31)𝑑𝜃1 + (𝑈1𝐽12 + 𝑈2𝐽22 + 𝑈3𝐽32)𝑑𝜃2 + (𝑈1𝐽13 + 𝑈2𝐽23 + 𝑈3𝐽33)𝑑𝜃3

Equation (1) held true in all circumstances. Thus the motor torques were given by:

𝜏1 = −(𝑇𝐴 + 𝑈1𝐽11 + 𝑈2𝐽21 + 𝑈3𝐽31)

𝜏2 = −(𝑇𝐵 + 𝑈1𝐽12 + 𝑈2𝐽22 + 𝑈3𝐽32)

𝜏3 = −(𝑇𝐶 + 𝑈1𝐽13 + 𝑈2𝐽23 + 𝑈3𝐽33)

2. Manipulator Jacobian

A. Inverse Kinematics

The length of the forearm was constant. Namely, ‖𝐵𝑖𝐶𝑖⃗⃗ ⃗⃗ ⃗⃗ ⃗‖ = 𝑙. Thus the following equations

could be written down: (the expressions of 𝐵𝑖𝐶𝑖⃗⃗ ⃗⃗ ⃗⃗ ⃗ could be found above, and let (𝑟 − 𝑅) = 𝐷.)

∥ 𝐵1𝐶1⃗⃗ ⃗⃗ ⃗⃗ ⃗⃗ ⃗ ∥2= 𝑙2 = (𝑥𝑝 + 𝐷 − 𝐿𝑐𝑜𝑠𝜃1)2+ 𝑦𝑝

2 + (𝑧𝑝 − 𝐿𝑠𝑖𝑛𝜃1)2

= 𝑥𝑝2 + 𝐷2 + 𝐿2 cos2 𝜃1 + 2𝑥𝑝𝐷 − 2𝑥𝑝𝐿𝑐𝑜𝑠𝜃1 − 2𝐷𝐿𝑐𝑜𝑠𝜃1 + 𝑦𝑝

2 + 𝑧𝑝2

+ 𝐿2 sin2 𝜃1 − 2𝐿𝑧𝑝𝑠𝑖𝑛𝜃1

(2)

⇒ −2𝐿(𝑥𝑝 + 𝐷)𝑐𝑜𝑠𝜃1 − 2𝐿𝑧𝑝𝑠𝑖𝑛𝜃1 + (𝑥𝑝2 + 𝑦𝑝

2 + 𝑧𝑝2 + 𝐷2 + 𝐿2 + 2𝑥𝑝𝐷 − 𝑙

2) = 0

⇒ 𝑀1𝑐𝑜𝑠𝜃1 + 𝑁1𝑠𝑖𝑛𝜃1 + 𝑂1 = 0

The second forearm:

∥ 𝐵2𝐶2⃗⃗ ⃗⃗ ⃗⃗ ⃗⃗ ⃗ ∥2= 𝑙2 = (𝑥𝑝 −1

2𝐷 +

1

2𝐿𝑐𝑜𝑠𝜃2)

2

+ (𝑦𝑝 +√3

2𝐷 −

√3

2𝐿𝑐𝑜𝑠𝜃2)

2

+ (𝑧𝑝 − 𝐿𝑠𝑖𝑛𝜃2)2

= 𝑥𝑝2 +

1

4𝐷2 +

1

4𝐿2 𝑐𝑜𝑠2 𝜃2 − 𝑥𝑝𝐷 + 𝑥𝑝𝐿𝑐𝑜𝑠𝜃2 −

1

2𝐷𝐿𝑐𝑜𝑠𝜃2 + 𝑦𝑝

2 +3

4𝐷2 +

3

4𝐿2 𝑐𝑜𝑠2 𝜃2

+ √3𝑦𝑝𝐷 − √3𝑦𝑝𝐿𝑐𝑜𝑠𝜃2 −3

2𝐷𝐿𝑐𝑜𝑠𝜃2 + 𝑧𝑝

2 + 𝐿2 𝑠𝑖𝑛2 𝜃2 − 2𝐿𝑧𝑝𝑠𝑖𝑛𝜃2

⇒ 𝐿 (𝑥𝑝 −1

2𝐷 − √3𝑦𝑝 −

3

2𝐷) 𝑐𝑜𝑠𝜃2 + (−2𝐿𝑧𝑝)𝑠𝑖𝑛𝜃2

+ (𝑥𝑝2 + 𝑦𝑝

2 + 𝑧𝑝2 + 𝐷2 + 𝐿2 − 𝑥𝑝𝐷 + √3𝑦𝑝𝐷 − 𝑙

2) = 0

⇒ 𝑀2𝑐𝑜𝑠𝜃2 + 𝑁2𝑠𝑖𝑛𝜃2 + 𝑂2 = 0

The third forearm:

∥ 𝐵3𝐶3⃗⃗ ⃗⃗ ⃗⃗ ⃗⃗ ⃗ ∥2= 𝑙2 = (𝑥𝑝 −1

2𝐷 +

1

2𝐿𝑐𝑜𝑠𝜃3)

2

+ (𝑦𝑝 −√3

2𝐷 +

√3

2𝐿𝑐𝑜𝑠𝜃3)

2

+ (𝑧𝑝 − 𝐿𝑠𝑖𝑛𝜃3)2

= 𝑥𝑝2 +

1

4𝐷2 +

1

4𝐿2 cos2 𝜃3 − 𝑥𝑝𝐷 + 𝑥𝑝𝐿𝑐𝑜𝑠𝜃3 −

1

2𝐷𝐿𝑐𝑜𝑠𝜃3 + 𝑦𝑝

2 +3

4𝐷2 +

3

4𝐿2 cos2 𝜃3

− √3𝑦𝑝𝐷 + √3𝑦𝑝𝐿𝑐𝑜𝑠𝜃3 −3

2𝐷𝐿𝑐𝑜𝑠𝜃3 + 𝑧𝑝

2 + 𝐿2 sin2 𝜃3 − 2𝐿𝑧𝑝𝑠𝑖𝑛𝜃3

⇒ 𝐿 (𝑥𝑝 −1

2𝐷 + √3𝑦𝑝 −

3

2𝐷) 𝑐𝑜𝑠𝜃3 + (−2𝐿𝑧𝑝)𝑠𝑖𝑛𝜃3

+ (𝑥𝑝2 + 𝑦𝑝

2 + 𝑧𝑝2 + 𝐷2 + 𝐿2 − 𝑥𝑝𝐷 − √3𝑦𝑝𝐷 − 𝑙

2) = 0

⇒ 𝑀3𝑐𝑜𝑠𝜃3 + 𝑁3𝑠𝑖𝑛𝜃3 + 𝑂3 = 0

For equations of the form 𝑀𝑐𝑜𝑠𝜃 + 𝑁𝑠𝑖𝑛𝜃 + 𝑂 = 0, tangent half-angle substitution could be

applied to find the solution. Let 𝑡 = tan (𝜃/2)

⇒ 𝑐𝑜𝑠𝜃 =1 − 𝑡2

1 + 𝑡2, 𝑠𝑖𝑛𝜃 =

2𝑡

1 + 𝑡2

⇒ 𝑀1 − 𝑡2

1 + 𝑡2+ 𝑁

2𝑡

1 + 𝑡2+𝑂 = 0

⇒ 𝑀(1 − 𝑡2) + 2𝑁𝑡 + 𝑂(1 + 𝑡2) = 0 ⇒ (𝑂 −𝑀)𝑡2 + 2𝑁𝑡 + (𝑀 + 𝑂) = 0

⇒ 𝑡 =−2𝑁 ± √4𝑁2 − 4(𝑂 −𝑀)(𝑂 +𝑀)

2(𝑂 −𝑀)=−𝑁 ± √𝑁2 − 𝑂2 +𝑀2

𝑂 −𝑀

(3)

(4)

(5)

⇒ 𝜃 = 2 tan−1 𝑡

Therefore, the solutions of the robot inverse kinematics were given by:

𝑡𝑖 =−𝑁𝑖 ±√𝑁𝑖

2 − 𝑂𝑖2 +𝑀𝑖

2

𝑂𝑖 −𝑀𝑖, 𝜃𝑖 = 2 tan

−1 𝑡𝑖

, where 𝑀𝑖, 𝑁𝑖 and 𝑂𝑖 were functions of (𝑥𝑝, 𝑦𝑝, 𝑧𝑝). The two solutions for the rotational angle

corresponded to the elbow-out and elbow-in configuration. (Fig.4)

Fig.4. The possible link configurations of the Delta robot: the elbow-out configurations were

represented by black lines (𝐴1 − 𝐵1 − 𝐶1), and the elbow-in configurations were represented by

blue lines (𝐴1 − 𝐵1′ − 𝐶1) for conditions (a) where an elbow could be above the XY plane, the robot

base, and (b) where all elbows were below the x-y plane.

Angle Determination Logic: depending on the location of the end effector, both the elbow-

out and elbow-in configuration could be under the XY plane (Fig.4b); another circumstance was

that the elbow-out configuration exceeded the XY plane but the elbow-in configuration remained

beneath it. (Fig.4a) Since the upper link was not allowed to rotate above the XY plane, the elbow-

out configuration in Fig.4a would not be accepted. For the same reason, the circumstance where

both the elbow-out and elbow-in configuration exceeded the XY plane had not been shown here.

In order to avoid singularities during robot operation, only the elbow-out configuration was

allowed in the process of searching the reachable workspace. (This statement would be discussed

in detail later.) Thus the rotational angle choice reduced from total six to one. (Elbow-out in Fig.4b)

B. Manipulator Jacobian

The manipulator Jacobian could be calculated by taking the time derivatives of the inverse

kinematics equations. Start from equation (3):

−2𝐿(𝑥�̇�𝑐𝑜𝑠𝜃1 − 𝑥𝑝𝑠𝑖𝑛𝜃1𝜃1̇) + 2𝐿𝐷𝑠𝑖𝑛𝜃1𝜃1̇ − 2𝐿(𝑧�̇�𝑠𝑖𝑛𝜃1 + 𝑧𝑝𝑐𝑜𝑠𝜃1𝜃1̇)

+(2𝑥𝑝𝑥�̇� + 2𝑦𝑝𝑦�̇� + 2𝑧𝑝𝑧�̇� + 2𝐷𝑥�̇�) = 0

Rewritten as:

(−2𝐿𝑐𝑜𝑠𝜃1 + 2𝑥𝑝 + 2𝐷)𝑥�̇� + (2𝑦𝑝)𝑦�̇� + (−2𝐿𝑠𝑖𝑛𝜃1 + 2𝑧𝑝)𝑧�̇�= −(2𝐿𝑥𝑝𝑠𝑖𝑛𝜃1 + 2𝐿𝐷𝑠𝑖𝑛𝜃1 − 2𝐿𝑧𝑝𝑐𝑜𝑠𝜃1)𝜃1̇

⇒ 𝑎11𝑥�̇� + 𝑎12𝑦�̇� + 𝑎13𝑧�̇� = 𝑏11𝜃1̇

The time derivative of equation (4):

𝐿(𝑥�̇�𝑐𝑜𝑠𝜃2 − 𝑥𝑝𝑠𝑖𝑛𝜃2𝜃2̇) +1

2𝐿𝐷𝑠𝑖𝑛𝜃2𝜃2̇ − √3𝐿(𝑦�̇�𝑐𝑜𝑠𝜃2 − 𝑦𝑝𝑠𝑖𝑛𝜃2𝜃2̇) +

3

2𝐿𝐷𝑠𝑖𝑛𝜃2𝜃2̇

− 2𝐿(𝑧�̇�𝑠𝑖𝑛𝜃2 + 𝑧𝑝𝑐𝑜𝑠𝜃2𝜃2̇) + (2𝑥𝑝𝑥�̇� + 2𝑦𝑝𝑦�̇� + 2𝑧𝑝𝑧�̇� − 𝐷𝑥�̇� + √3𝐷𝑦�̇�) = 0

Rewritten as:

(𝐿𝑐𝑜𝑠𝜃2 + 2𝑥𝑝 − 𝐷)𝑥�̇� + (−√3𝐿𝑐𝑜𝑠𝜃2 + 2𝑦𝑝 + √3𝐷)𝑦�̇� + (−2𝐿𝑠𝑖𝑛𝜃2 + 2𝑧𝑝)𝑧�̇�

= −(−𝐿𝑥𝑝𝑠𝑖𝑛𝜃2 + √3𝐿𝑦𝑝𝑠𝑖𝑛𝜃2 + 2𝐿𝐷𝑠𝑖𝑛𝜃2 − 2𝐿𝑧𝑝𝑐𝑜𝑠𝜃2)𝜃2̇

⇒ 𝑎21𝑥�̇� + 𝑎22𝑦�̇� + 𝑎23𝑧�̇� = 𝑏22𝜃2̇

The time derivative of equation (5):

𝐿(𝑥�̇�𝑐𝑜𝑠𝜃3 − 𝑥𝑝𝑠𝑖𝑛𝜃3𝜃3̇) +1

2𝐿𝐷𝑠𝑖𝑛𝜃3𝜃3̇ + √3𝐿(𝑦�̇�𝑐𝑜𝑠𝜃3 − 𝑦𝑝𝑠𝑖𝑛𝜃3𝜃3̇) +

3

2𝐿𝐷𝑠𝑖𝑛𝜃3𝜃3̇

− 2𝐿(𝑧�̇�𝑠𝑖𝑛𝜃3 + 𝑧𝑝𝑐𝑜𝑠𝜃3𝜃3̇) + (2𝑥𝑝𝑥�̇� + 2𝑦𝑝𝑦�̇� + 2𝑧𝑝𝑧�̇� − 𝐷𝑥�̇� − √3𝐷𝑦�̇�) = 0

Rewritten as:

(𝐿𝑐𝑜𝑠𝜃3 + 2𝑥𝑝 − 𝐷)𝑥�̇� + (√3𝐿𝑐𝑜𝑠𝜃3 + 2𝑦𝑝 − √3𝐷)𝑦�̇� + (−2𝐿𝑠𝑖𝑛𝜃3 + 2𝑧𝑝)𝑧�̇�

= −(−𝐿𝑥𝑝𝑠𝑖𝑛𝜃3 − √3𝐿𝑦𝑝𝑠𝑖𝑛𝜃3 + 2𝐿𝐷𝑠𝑖𝑛𝜃3 − 2𝐿𝑧𝑝𝑐𝑜𝑠𝜃3)𝜃3̇

⇒ 𝑎31𝑥�̇� + 𝑎32𝑦�̇� + 𝑎33𝑧�̇� = 𝑏33𝜃3̇

Thus, the manipulator Jacobian could be written as:

[

𝑎11 𝑎12 𝑎13𝑎21 𝑎22 𝑎23𝑎31 𝑎32 𝑎33

] [

𝑥�̇�𝑦�̇�𝑧�̇�

] = [

𝑏11 0 00 𝑏22 00 0 𝑏33

] [

𝜃1̇𝜃2̇𝜃3̇

] ⇒ 𝐀�̇� = 𝑩�̇� ⇒ 𝑱 = 𝑨−𝟏𝑩

C. Motor Angular Acceleration

The angular acceleration could be found by taking the derivative of the Jacobian equation.

𝑑

𝑑𝑡(𝐀�̇�) =

𝑑

𝑑𝑡(𝑩�̇�)

�̇��̇� + 𝑨�̈� = �̇��̇� + 𝑩�̈�

⇒ �̈� = 𝑩−𝟏(�̇��̇� + 𝑨�̈� − �̇��̇�)

3. The Workspace Search Algorithm

Starting from a point in space, if the end effector could move at the given speed |𝑣| in any

direction, its velocity vector could reach any point on a sphere with radius 𝑟 = |𝑣|. (Here the |𝑣| was the task requirement set by the user.) If the velocity vector could reach the eight vertices of

the cube that circumscribed the sphere, it could reach any point on the cube and thus any point on

the sphere. (Fig.5) The coordinates of the vertices were given by:

𝒗 = [

|𝑣 | |𝑣 | |𝑣 | |𝑣 | −|𝑣 | −|𝑣 | −|𝑣 | −|𝑣 |

|𝑣 | |𝑣 | −|𝑣 | −|𝑣 | |𝑣 | |𝑣 | −|𝑣 | −|𝑣 |

|𝑣 | −|𝑣 | |𝑣 | −|𝑣 | |𝑣 | −|𝑣 | |𝑣 | −|𝑣 |

]

, where each column represented a point that the velocity vector must be able to reach. The

acceleration vertices could be formulated in the same way to generate a 3 × 8 matrix.

Fig.5. The velocity sphere of the moving platform and its circumscribed cube.

At each point in the reachable workspace, 8 × 8 = 64 combinations of moving platform velocity

and acceleration (corresponded to the cube vertices) were substituted into the torque equation and

the Jacobian equation. Then, the maximum value of the torque and angular speed calculated was

used to compare with the parameters of the actual motor installed. If the motor parameter exceeded

this maximum value, this point was inside the dynamic workspace. In other words, if the motor

could operate at the angular speed faster than the calculated maximum, and at the same time output

the torque larger than the calculated maximum, the end effector could reach all the vertices on the

velocity and acceleration cube; thus it could move at the given 𝑣 and 𝑎 of any direction.

The reachable workspace was the region where the kinematics equations had a real

solution. Also, the 𝜃𝑖 < 0 condition and the elbow-out configuration both needed to be satisfied.

III. Results and Analysis

1. The Reachable Workspace

The reachable workspace contained all the locations the moving platform could arrive. The

kinematics equations indicated that this workspace was only impacted by the geometric size of the

robot. By modifying the robot arm length and keeping the size of the base and moving platform

constant, the following reachable workspaces could be established: (Fig.6)

Fig.6. The reachable workspaces for three combinations of upper arm and forearm of different

lengths. R = 0.06m, r = 0.017m.

As the robot arm length increased, the reachable workspace expanded, which meant the

end effector could span larger volume in space. The user could substitute various length values

(and/or base and moving platform size values) to find the appropriate reachable workspace for his

task requirement.

2. The Workspace with Angular Velocity Limits

From the manipulator Jacobian equation, it was shown that the angular speed of the motors

determined the spatial velocity of the end effector. By setting a limit on the motor rotational speed,

some regions inside the reachable workspace would be expunged since the motor could not provide

the required rotational speed for the end effector to have the ability to move in any direction in

these regions with the given (maximum) speed. The following workspace plot reflected the

influence of the motor RPM limit: (Fig.7)

Fig.7. The workspaces where the moving platform could move at |𝑣| = 1m/s in any direction.

L = 0.055m, l = 0.06m, R = 0.06m, r = 0.017m.

As the motor rotational speed limit decreased, the region where the end effector could move

in any direction at the given speed reduced. This region was referred to as the velocity workspace.

3. The Workspace with Torque Limits.

In the torque equations, the moving platform acceleration and the motor torques were

coupled together, which indicated that the motor torque set a limit for the region where the end

effector could move in any direction with the given (maximum) acceleration. Since the motor

rotational speed also appeared in the torque equation, one temporary assumption needed to be

made to allow the torque calculation: the motor could provide any rotational speed at the position

where the algorithm tested the torque requirement. The following workspace plot reflected the

influence of the motor torque limit: (Fig.8)

Fig.8. The workspaces where the moving platform could move at |𝑎| = 1m/s2 in any direction.

L = 0.055m, l = 0.06m, R = 0.06m, r = 0.017m, mL = 21.3g, ml = 5.8g, mP = 250g.

The workspace in Fig.8 represented the region where the end effector could move in any

direction with the given acceleration, which was constrained by the maximum torque output of the

motor. This region was referred to as the acceleration workspace.

4. The Dynamic Workspace

In the process of constructing the acceleration workspace, the assumption that the motor

could provide any rotational speed inside this workspace was made. Since the actual motor had a

limitation on the RPM, in some regions, this assumption would not be valid. However, these

regions could be excluded by overlapping the velocity workspace onto the acceleration workspace.

(Fig.9) Inside the overlapped region, the motor could rotate at the angular speed less than its

limitation and at the same time produce the torque required. In other words, inside this overlapped

region, the end effector could move with any combination of 𝑣 and 𝑎 given, as long as they did

not exceed the user-defined limits. This region was referred to as the dynamic workspace.

Fig.9. The dynamic workspace obtained by overlapping the green velocity space and the red

acceleration workspace together. Inside the overlapped region, the end effector could move at

|𝑣| = 1m/s and |𝑎| = 1m/s2 in any direction. Robot information same as that of Fig.8.

The region outside the dynamic workspace indicated one or more conditions had not been

met. For example, the region covered by the velocity workspace but not the acceleration workspace

meant that although the motor could provide the RPM required for the end effector to move with

any 𝑣 , it could not at the same time provide the torque required for the end effector to move with

any 𝑎 in its motion. By modifying the two limits (torque and angular speed), the user could

generate a dynamic workspace that covered the task requirement space, and the data (geometric

size, motor limits, link mass, etc.) could be used to aid the robot selection.

5. The GUI in Design Analysis

The efficiency of the algorithm could be improved by creating a GUI with torque limit and

angular velocity limit as input sliders. (Fig.10) Since the no-load speed and stall torque were

usually used in the motor’s technical datasheets to illustrate its capacity, for convenience, the

sliders would take these two values as input, and the actual torque and angular velocity limit in the

background calculation were chosen to be half of them, where the motor mechanical power output

reached the maximum. (Fig.11)

Fig.10. The GUI example. By dragging the two sliders, the user could set the stall torque and no-

load speed (to match with those of the actual motors). Then, click the Run button to generate a

dynamic workspace plot.

Fig.11. The motor torque-speed curve and the mechanical power curve. Point P (midpoint)

represented the actual torque and angular velocity limit values used in the calculation after the user

inputted the no-load speed and stall torque.

The primal purpose of the GUI was to rapidly compare the dynamic workspace for different

types of motor. In the following example, for the same Delta robot, three types of motor (Fig.12,

Table.1) produced various dynamics workspaces. (Fig.14) The robot geometric size and mass data

remained same as those of Fig.8 except now mP = 50g. These robot data were obtained from a

specific handheld 3D bio-printing Delta robot designed for medical applications in the Medical

Robotics and Devices Lab. (Fig.13)

Fig.12. The motors listed in Table 1.

Motor model No-load speed

[sec/60°] [rad/s]

Stall torque

[oz/in.] [Nm]

Retail price

[USD]

Futaba S9470SV 0.09 11.64 191.7 1.35 99.99

Savox SA-1258TG 0.08 13.09 166.6 1.18 59.99

Futaba S3003 0.19 5.51 56.8 0.40 10.99

Table.1. Motor technical specifications selection

Fig.13. The handheld 3D bio-printing Delta robot

Fig.14. The comparison of the dynamic workspaces generated from three types of motors. The T

in the legend represented the torque limit and the AV for the angular velocity limit. (Half of the

stall torque and no-load speed) The UI control buttons had been hided.

From Table 1, the Futaba S9470SV and Savox SA-1258TG motor had similar stall torque

and no-load speed. Thus the dynamic workspaces for them nearly coincided with each other. As a

comparison group, the Futaba S3003 motor provided significantly lower stall torque and no-load

speed. Therefore its corresponding dynamic workspace occupied much less volume than the other

two. Depending on the task requirement, the user could choose suitable motor to reduce cost.

6. The Role of Singularities

In the discussion of the motor rotational angle determination logic (Section II.2.A), it was

mentioned that only the elbow-out configuration in Fig.4b was accepted. If the elbow-in

configuration had also been allowed, the reachable workspace would have expanded greatly.

(Fig.15) However, if both the elbow-out and elbow-in configurations were allowed, there would

be a transition point where the upper arm and forearm formed a straight line. In this circumstance,

this link group fell into the singularity where det(J) = 0. Robot operating near the singularities

could trigger unpredictable consequences, such as losing degrees of freedom, requiring infinite

motor torque, etc. Therefore, to avoid singularities during gesture transition, the elbow-in

configuration had been expunged from the construction of the robot reachable workspace.

Fig.15. The reachable workspace comparison. The green workspace allowed both the elbow-out

and elbow-in configurations. Inside the red workspace (elbow-out configuration only), the moving

platform would not experience any singularities. R = 0.06m, r = 0.017m

While the selection of the elbow-out configuration was a specific design choice which

followed the common practice in industry, the equations (torque, manipulator Jacobian) held true

in all circumstances (both elbow-in and elbow-out).

IV. Conclusion

The dynamic solutions of the Delta robot have been derived in detail. The structure of the

reachable workspace and the dynamic workspace as a function of robot size and motor

performance specifications was visualized for a variety of practical situations. The MATLAB

toolkit with GUI provided a convenient way to compare the dynamic workspace for different

motors, enabling future users to visualize dynamic workspaces for any practical robot dimensions

or motor specifications. The dynamic workspace evaluation for a specific Delta robot designed for

medical application had been conducted and the motor influence were shown. Based on the

dynamic solutions derived and the workspace construct algorithm provided, a more user-friendly

robot design toolkit that took all the robot parameters (size, link mass, motor data, etc.) as GUI

inputs and with better (near simultaneous) calculation and graphic plotting performance could be

expected in future.

References:

[1] R.L. Williams II, “The Delta Parallel Robot: Kinematics Solutions”, Internet Publication,

www.ohio.edu/people/williar4/html/pdf/DeltaKin.pdf, January 2016.

[2] López, M., Castillo, E., García, G. and Bashir, A. (2006). Delta robot: Inverse, direct, and

intermediate Jacobians. Proceedings of the Institution of Mechanical Engineers, Part C: Journal of

Mechanical Engineering Science, 220(1), pp.103-109.

[3] P. Guglielmetti and R. Longchamp, "A closed form inverse dynamics model of the delta

parallel robot", IFAC Proceedings Volumes, vol. 27, no. 14, pp. 51-56, 1994.

[4] P. Guglielmetti and R. Longchamp, "Task Space Control of the Delta Parallel Robot", IFAC

Proceedings Volumes, vol. 25, no. 29, pp. 337-342, 1992.

[5] Staicu, Stefan & Carp-Ciocardia, Daniela-Craita. (2003). Dynamic analysis of Clavel’s Delta

parallel robot. Proceedings - IEEE International Conference on Robotics and Automation. 3.

10.1109/ROBOT.2003.1242230.

[6] Zhao, Y & Yang, Z & Huang, Taosheng. (2005). Inverse dynamics of delta robot based on the

principle of virtual work. Transactions of Tianjin University. 11. 268-273.

[7] Codourey, A & Burdet, Etienne. (1997). Body-oriented method for finding a linear form of the

dynamic equation of fully parallel robots. Proceedings - IEEE International Conference on

Robotics and Automation. 2. 1612 - 1618 vol.2. 10.1109/ROBOT.1997.614371.

[8] L. Tsai, "Solving the Inverse Dynamics of a Stewart-Gough Manipulator by the Principle of

Virtual Work", Journal of Mechanical Design, vol. 122, no. 1, p. 3, 2000.

Appendix

1. The Link Group 2 (𝑨𝟐 −𝑩𝟐 − 𝑪𝟐) Dynamics:

𝐴2 = (−1

2𝑅,√3

2𝑅, 0) 𝐵2 = (−

1

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃2),

√3

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃2), 𝐿𝑠𝑖𝑛𝜃2)

𝑃 = (𝑥𝑝, 𝑦𝑝, 𝑧𝑝) 𝐶2 = (𝑥𝑝 −1

2𝑟, 𝑦𝑝 +

√3

2𝑟, 𝑧𝑝)

𝑀2 =𝐴2 + 𝐵22

= (−1

4(2𝑅 + 𝐿𝑐𝑜𝑠𝜃2),

√3

4(2𝑅 + 𝐿𝑐𝑜𝑠𝜃2),

1

2𝐿𝑠𝑖𝑛𝜃2)

𝑁2 = (1

2(−1

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃2) + 𝑥𝑝 −

1

2𝑟) ,

1

2(√3

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃2) + 𝑦𝑝 +

√3

2𝑟) ,

1

2(𝐿𝑠𝑖𝑛𝜃2 + 𝑧𝑝))

𝑑

𝑑𝑡𝑂𝑀2⃗⃗ ⃗⃗ ⃗⃗ ⃗⃗ ⃗ = (−

1

4(−𝐿𝑠𝑖𝑛𝜃2 ∙ 𝜃2̇),

√3

4(−𝐿𝑠𝑖𝑛𝜃2 ∙ 𝜃2̇),

1

2𝐿𝑐𝑜𝑠𝜃2 ∙ 𝜃2̇)

𝑑

𝑑𝑡𝑂𝑁2⃗⃗ ⃗⃗ ⃗⃗ ⃗⃗ = (

1

2(1

2𝐿𝑠𝑖𝑛𝜃2 ∙ 𝜃2̇ + 𝑥�̇�) ,

1

2(−√3

2𝐿𝑠𝑖𝑛𝜃2 ∙ 𝜃2̇ + 𝑦�̇�) ,

1

2(𝐿𝑐𝑜𝑠𝜃2 ∙ 𝜃2̇ + 𝑧�̇�))

𝐵2𝐶2⃗⃗ ⃗⃗ ⃗⃗ ⃗⃗ ⃗ = (𝑥𝑝 −1

2𝑟 +

1

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃2), 𝑦𝑝 +

√3

2𝑟 −

√3

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃2), 𝑧𝑝 − 𝐿𝑠𝑖𝑛𝜃2)

𝑣𝐶2⃗⃗⃗⃗⃗⃗ =𝑑

𝑑𝑡𝐵2𝐶2⃗⃗ ⃗⃗ ⃗⃗ ⃗⃗ ⃗ = (𝑥�̇� −

1

2𝐿𝑠𝑖𝑛𝜃2 ∙ 𝜃2̇, 𝑦�̇� +

√3

2𝐿𝑠𝑖𝑛𝜃2 ∙ 𝜃2̇, 𝑧�̇� − 𝐿𝑐𝑜𝑠𝜃2 ∙ 𝜃2̇)

�⃗⃗� =𝑟 × 𝑣

𝑟2

𝑑

𝑑𝑡𝜃𝐵2 = 𝜔𝐵2⃗⃗ ⃗⃗ ⃗⃗ ⃗ =

1

𝑙2(𝐵2𝐶2⃗⃗ ⃗⃗ ⃗⃗ ⃗⃗ ⃗ × 𝑣𝐶2⃗⃗⃗⃗⃗⃗ )

=1

𝑙2

(

(𝑦𝑝 +

√3

2𝑟 −

√3

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃2)) (𝑧�̇� − 𝐿𝑐𝑜𝑠𝜃2 ∙ 𝜃2̇) − (𝑧𝑝 − 𝐿𝑠𝑖𝑛𝜃2) (𝑦�̇� +

√3

2𝐿𝑠𝑖𝑛𝜃2 ∙ 𝜃2̇) ,

(𝑧𝑝 − 𝐿𝑠𝑖𝑛𝜃2) (𝑥�̇� −1

2𝐿𝑠𝑖𝑛𝜃2 ∙ 𝜃2̇) − (𝑥𝑝 −

1

2𝑟 +

1

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃2)) (𝑧�̇� − 𝐿𝑐𝑜𝑠𝜃2 ∙ 𝜃2̇),

(𝑥𝑝 −1

2𝑟 +

1

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃2))(𝑦�̇� +

√3

2𝐿𝑠𝑖𝑛𝜃2 ∙ 𝜃2̇) − (𝑦𝑝 +

√3

2𝑟 −

√3

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃2))(𝑥�̇� −

1

2𝐿𝑠𝑖𝑛𝜃2 ∙ 𝜃2̇)

)

Let

𝐾2𝑠 = 𝐿𝑠𝑖𝑛𝜃2 𝐾2𝑐 = 𝐿𝑐𝑜𝑠𝜃2

𝑑𝜃𝐵2

=1

𝑙2

(

(𝑦𝑝 +

√3

2𝑟 −

√3

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃2)) (𝑑𝑧𝑝 − 𝐿𝑐𝑜𝑠𝜃2 ∙ 𝑑𝜃2) − (𝑧𝑝 − 𝐿𝑠𝑖𝑛𝜃2) (𝑑𝑦𝑝 +

√3

2𝐿𝑠𝑖𝑛𝜃2 ∙ 𝑑𝜃2) ,

(𝑧𝑝 − 𝐿𝑠𝑖𝑛𝜃2) (𝑑𝑥𝑝 −1

2𝐿𝑠𝑖𝑛𝜃2 ∙ 𝑑𝜃2) − (𝑥𝑝 −

1

2𝑟 +

1

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃2)) (𝑑𝑧𝑝 − 𝐿𝑐𝑜𝑠𝜃2 ∙ 𝑑𝜃2),

(𝑥𝑝 −1

2𝑟 +

1

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃2))(𝑑𝑦𝑝 +

√3

2𝐿𝑠𝑖𝑛𝜃2 ∙ 𝑑𝜃2) − (𝑦𝑝 +

√3

2𝑟 −

√3

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃2))(𝑑𝑥𝑝 −

1

2𝐿𝑠𝑖𝑛𝜃2 ∙ 𝑑𝜃2)

)

=

(

𝐺1(𝑑𝑧𝑝 − 𝐾2𝑐 ∙ 𝑑𝜃2) − 𝐺2 (𝑑𝑦𝑝 +

√3

2𝐾2𝑠 ∙ 𝑑𝜃2) ,

𝐺2 (𝑑𝑥𝑝 −1

2𝐾2𝑠 ∙ 𝑑𝜃2) − 𝐺3(𝑑𝑧𝑝 − 𝐾2𝑐 ∙ 𝑑𝜃2),

𝐺3 (𝑑𝑦𝑝 +√3

2𝐾2𝑠 ∙ 𝑑𝜃2) − 𝐺1 (𝑑𝑥𝑝 −

1

2𝐾2𝑠 ∙ 𝑑𝜃2)

)

∑𝑊𝐿𝐺2 = 𝑊𝐴2𝐵2 +𝑊𝐵2𝐶2

= (𝑚𝐿𝑔 ∙ 𝛿𝑀2𝑧 + 𝐼𝐿𝜃2̈ ∙ 𝑑𝜃2) + (𝑚𝑙𝑔 ∙ 𝛿𝑁2𝑧 +𝑚𝑙𝑎𝑁2 ∙ 𝛿𝑁2 + 𝐼𝑙𝜔𝐵2̇ ∙ 𝑑𝜃𝐵2)

The acceleration at point 𝑁2 and the angular acceleration of link 𝐵2𝐶2 were represented by:

𝑎𝑁2 =𝑑

𝑑𝑡(𝑑

𝑑𝑡𝑂𝑁2⃗⃗ ⃗⃗ ⃗⃗ ⃗⃗ ) = (𝑎𝑁2𝑥, 𝑎𝑁2𝑦, 𝑎𝑁2𝑧)

𝜔𝐵2̇ =𝑑

𝑑𝑡(𝜔𝐵2⃗⃗ ⃗⃗ ⃗⃗ ⃗) = (𝜔𝐵2𝑥̇ , 𝜔𝐵2𝑦̇ , 𝜔𝐵2𝑧̇ )

𝛿𝑀2𝑧 =1

2𝐿𝑐𝑜𝑠𝜃2 ∙ 𝑑𝜃2 =

1

2𝐾2𝑐 ∙ 𝑑𝜃2

𝛿𝑁2 = (1

2(1

2𝐿𝑠𝑖𝑛𝜃2 ∙ 𝑑𝜃2 + 𝑑𝑥𝑝) ,

1

2(−√3

2𝐿𝑠𝑖𝑛𝜃2 ∙ 𝑑𝜃2 + 𝑑𝑦𝑝) ,

1

2(𝐿𝑐𝑜𝑠𝜃2 ∙ 𝑑𝜃2 + 𝑑𝑧𝑝))

= (1

2(1

2𝐾2𝑠 ∙ 𝑑𝜃2 + 𝑑𝑥𝑝) ,

1

2(−√3

2𝐾2𝑠 ∙ 𝑑𝜃2 + 𝑑𝑦𝑝) ,

1

2(𝐾2𝑐 ∙ 𝑑𝜃2 + 𝑑𝑧𝑝))

𝛿𝑁2𝑧 =1

2(𝐿𝑐𝑜𝑠𝜃2 ∙ 𝑑𝜃2 + 𝑑𝑧𝑝) =

1

2(𝐾2𝑐 ∙ 𝑑𝜃2 + 𝑑𝑧𝑝)

𝑊𝐴2𝐵2 +𝑊𝐵2𝐶2 = 𝑚𝐿𝑔 ∙1

2𝐾2𝑐 ∙ 𝑑𝜃2 + 𝐼𝐿𝜃2̈ ∙ 𝑑𝜃2 +𝑚𝑙𝑔 ∙

1

2(𝐾2𝑐 ∙ 𝑑𝜃2 + 𝑑𝑧𝑝)

+𝑚𝑙 [𝑎𝑁2𝑥 (1

2(1

2𝐾2𝑠 ∙ 𝑑𝜃2 + 𝑑𝑥𝑝)) + 𝑎𝑁2𝑦 (

1

2(−√3

2𝐾2𝑠 ∙ 𝑑𝜃2 + 𝑑𝑦𝑝))

+ 𝑎𝑁2𝑧 (1

2(𝐾2𝑐 ∙ 𝑑𝜃2 + 𝑑𝑧𝑝))]

+ 𝐼𝑙 [𝜔𝐵2𝑥̇ (𝐺1(𝑑𝑧𝑝 − 𝐾2𝑐 ∙ 𝑑𝜃2) − 𝐺2 (𝑑𝑦𝑝 +√3

2𝐾2𝑠 ∙ 𝑑𝜃2))

+ 𝜔𝐵2𝑦̇ (𝐺2 (𝑑𝑥𝑝 −1

2𝐾2𝑠 ∙ 𝑑𝜃2) − 𝐺3(𝑑𝑧𝑝 − 𝐾2𝑐 ∙ 𝑑𝜃2))

+ 𝜔𝐵2𝑧̇ (𝐺3 (𝑑𝑦𝑝 +√3

2𝐾2𝑠 ∙ 𝑑𝜃2) − 𝐺1 (𝑑𝑥𝑝 −

1

2𝐾2𝑠 ∙ 𝑑𝜃2))]

= (𝑚𝐿𝑔 ∙1

2𝐾2𝑐 + 𝐼𝐿𝜃2̈ +𝑚𝑙𝑔 ∙

1

2𝐾2𝑐 +𝑚𝑙𝑎𝑁2𝑥 ∙

1

4𝐾2𝑠 −𝑚𝑙𝑎𝑁2𝑦 ∙

√3

4𝐾2𝑠 +𝑚𝑙𝑎𝑁2𝑧 ∙

1

2𝐾2𝑐

− 𝐼𝑙𝜔𝐵2𝑥̇ 𝐺1𝐾2𝑐 − 𝐼𝑙𝜔𝐵2𝑥̇ 𝐺2 ∙√3

2𝐾2𝑠 − 𝐼𝑙𝜔𝐵2𝑦̇ 𝐺2 ∙

1

2𝐾2𝑠 + 𝐼𝑙𝜔𝐵2𝑦̇ 𝐺3𝐾2𝑐

+ 𝐼𝑙𝜔𝐵2𝑧̇ 𝐺3 ∙√3

2𝐾2𝑠 + 𝐼𝑙𝜔𝐵2𝑧̇ 𝐺1 ∙

1

2𝐾2𝑠)𝑑𝜃2

+ (𝑚𝑙𝑎𝑁2𝑥 ∙1

2+ 𝐼𝑙𝜔𝐵2𝑦̇ 𝐺2 − 𝐼𝑙𝜔𝐵2𝑧̇ 𝐺1)𝑑𝑥𝑝

+ (𝑚𝑙𝑎𝑁2𝑦 ∙1

2− 𝐼𝑙𝜔𝐵2𝑥̇ 𝐺2 + 𝐼𝑙𝜔𝐵2𝑧̇ 𝐺3)𝑑𝑦𝑝

+ (𝑚𝑙𝑎𝑁2𝑧 ∙1

2+ 𝐼𝑙𝜔𝐵2𝑥̇ 𝐺1 − 𝐼𝑙𝜔𝐵2𝑦̇ 𝐺3 +𝑚𝑙𝑔 ∙

1

2) 𝑑𝑧𝑝

= 𝑇𝐵𝑑𝜃2 + 𝑇21𝑑𝑥𝑝 + 𝑇22𝑑𝑦𝑝 + 𝑇23𝑑𝑧𝑝

2. The Link Group 3 (𝑨𝟑 −𝑩𝟑 − 𝑪𝟑) Dynamics:

𝐴3 = ( −1

2𝑅,−

√3

2𝑅, 0) 𝐵3 = (−

1

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃3),−

√3

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃3), 𝐿𝑠𝑖𝑛𝜃3)

𝑃 = (𝑥𝑝, 𝑦𝑝, 𝑧𝑝) 𝐶3 = (𝑥𝑝 −1

2𝑟, 𝑦𝑝 −

√3

2𝑟, 𝑧𝑝)

𝑀3 =𝐴3 + 𝐵32

= (−1

4(2𝑅 + 𝐿𝑐𝑜𝑠𝜃3), −

√3

4(2𝑅 + 𝐿𝑐𝑜𝑠𝜃3),

1

2𝐿𝑠𝑖𝑛𝜃3)

𝑁3 = (1

2(−1

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃3) + 𝑥𝑝 −

1

2𝑟) ,

1

2(−√3

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃3) + 𝑦𝑝 −

√3

2𝑟) ,

1

2(𝐿𝑠𝑖𝑛𝜃3 + 𝑧𝑝))

𝑑

𝑑𝑡𝑂𝑀3⃗⃗ ⃗⃗ ⃗⃗ ⃗⃗ ⃗ = (−

1

4(−𝐿𝑠𝑖𝑛𝜃3 ∙ 𝜃3̇), −

√3

4(−𝐿𝑠𝑖𝑛𝜃3 ∙ 𝜃3̇),

1

2𝐿𝑐𝑜𝑠𝜃3 ∙ 𝜃3̇)

𝑑

𝑑𝑡𝑂𝑁3⃗⃗ ⃗⃗ ⃗⃗ ⃗⃗ = (

1

2(1

2𝐿𝑠𝑖𝑛𝜃3 ∙ 𝜃3̇ + 𝑥�̇�) ,

1

2(√3

2𝐿𝑠𝑖𝑛𝜃3 ∙ 𝜃3̇ + 𝑦�̇�) ,

1

2(𝐿𝑐𝑜𝑠𝜃3 ∙ 𝜃3̇ + 𝑧�̇�))

𝐵3𝐶3⃗⃗ ⃗⃗ ⃗⃗ ⃗⃗ ⃗ = (𝑥𝑝 −1

2𝑟 +

1

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃3), 𝑦𝑝 −

√3

2𝑟 +

√3

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃3), 𝑧𝑝 − 𝐿𝑠𝑖𝑛𝜃3)

𝑣𝐶3⃗⃗⃗⃗⃗⃗ =𝑑

𝑑𝑡𝐵3𝐶3⃗⃗ ⃗⃗ ⃗⃗ ⃗⃗ ⃗ = (𝑥�̇� −

1

2𝐿𝑠𝑖𝑛𝜃3 ∙ 𝜃3̇, 𝑦�̇� −

√3

2𝐿𝑠𝑖𝑛𝜃3 ∙ 𝜃3̇, 𝑧�̇� − 𝐿𝑐𝑜𝑠𝜃3 ∙ 𝜃3̇)

�⃗⃗� =𝑟 × 𝑣

𝑟2

𝑑

𝑑𝑡𝜃𝐵3 = 𝜔𝐵3⃗⃗ ⃗⃗ ⃗⃗ ⃗ =

1

𝑙2(𝐵3𝐶3⃗⃗ ⃗⃗ ⃗⃗ ⃗⃗ ⃗ × 𝑣𝐶3⃗⃗⃗⃗⃗⃗ )

=1

𝑙2

(

(𝑦𝑝 −

√3

2𝑟 +

√3

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃3)) (𝑧�̇� − 𝐿𝑐𝑜𝑠𝜃3 ∙ 𝜃3̇) − (𝑧𝑝 − 𝐿𝑠𝑖𝑛𝜃3) (𝑦�̇� −

√3

2𝐿𝑠𝑖𝑛𝜃3 ∙ 𝜃3̇) ,

(𝑧𝑝 − 𝐿𝑠𝑖𝑛𝜃3) (𝑥�̇� −1

2𝐿𝑠𝑖𝑛𝜃3 ∙ 𝜃3̇) − (𝑥𝑝 −

1

2𝑟 +

1

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃3)) (𝑧�̇� − 𝐿𝑐𝑜𝑠𝜃3 ∙ 𝜃3̇),

(𝑥𝑝 −1

2𝑟 +

1

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃3))(𝑦�̇� −

√3

2𝐿𝑠𝑖𝑛𝜃3 ∙ 𝜃3̇) − (𝑦𝑝 −

√3

2𝑟 +

√3

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃3))(𝑥�̇� −

1

2𝐿𝑠𝑖𝑛𝜃3 ∙ 𝜃3̇)

)

Let

𝐾3𝑠 = 𝐿𝑠𝑖𝑛𝜃3 𝐾3𝑐 = 𝐿𝑐𝑜𝑠𝜃3

𝑑𝜃𝐵3

=1

𝑙2

(

(𝑦𝑝 −

√3

2𝑟 +

√3

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃3)) (𝑑𝑧𝑝 − 𝐿𝑐𝑜𝑠𝜃3 ∙ 𝑑𝜃3) − (𝑧𝑝 − 𝐿𝑠𝑖𝑛𝜃3) (𝑑𝑦𝑝 −

√3

2𝐿𝑠𝑖𝑛𝜃3 ∙ 𝑑𝜃3) ,

(𝑧𝑝 − 𝐿𝑠𝑖𝑛𝜃3) (𝑑𝑥𝑝 −1

2𝐿𝑠𝑖𝑛𝜃3 ∙ 𝑑𝜃3) − (𝑥𝑝 −

1

2𝑟 +

1

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃3)) (𝑑𝑧𝑝 − 𝐿𝑐𝑜𝑠𝜃3 ∙ 𝑑𝜃3),

(𝑥𝑝 −1

2𝑟 +

1

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃3))(𝑑𝑦𝑝 −

√3

2𝐿𝑠𝑖𝑛𝜃3 ∙ 𝑑𝜃3) − (𝑦𝑝 −

√3

2𝑟 +

√3

2(𝑅 + 𝐿𝑐𝑜𝑠𝜃3))(𝑑𝑥𝑝 −

1

2𝐿𝑠𝑖𝑛𝜃3 ∙ 𝑑𝜃3)

)

=

(

𝐻1(𝑑𝑧𝑝 − 𝐾3𝑐 ∙ 𝑑𝜃3) − 𝐻2 (𝑑𝑦𝑝 −

√3

2𝐾3𝑠 ∙ 𝑑𝜃3) ,

𝐻2 (𝑑𝑥𝑝 −1

2𝐾3𝑠 ∙ 𝑑𝜃3) − 𝐻3(𝑑𝑧𝑝 − 𝐾3𝑐 ∙ 𝑑𝜃3),

𝐻3 (𝑑𝑦𝑝 −√3

2𝐾3𝑠 ∙ 𝑑𝜃3) − 𝐻1 (𝑑𝑥𝑝 −

1

2𝐾3𝑠 ∙ 𝑑𝜃3)

)

∑𝑊𝐿𝐺3 = 𝑊𝐴3𝐵3 +𝑊𝐵3𝐶3

= (𝑚𝐿𝑔 ∙ 𝛿𝑀3𝑧 + 𝐼𝐿𝜃3̈ ∙ 𝑑𝜃3) + (𝑚𝑙𝑔 ∙ 𝛿𝑁3𝑧 +𝑚𝑙𝑎𝑁3 ∙ 𝛿𝑁3 + 𝐼𝑙𝜔𝐵3̇ ∙ 𝑑𝜃𝐵3)

The acceleration at point 𝑁3 and the angular acceleration of link 𝐵3𝐶3 were represented by:

𝑎𝑁3 =𝑑

𝑑𝑡(𝑑

𝑑𝑡𝑂𝑁3⃗⃗ ⃗⃗ ⃗⃗ ⃗⃗ ) = (𝑎𝑁3𝑥, 𝑎𝑁3𝑦, 𝑎𝑁3𝑧)

𝜔𝐵3̇ =𝑑

𝑑𝑡(𝜔𝐵3⃗⃗ ⃗⃗ ⃗⃗ ⃗) = (𝜔𝐵3𝑥̇ , 𝜔𝐵3𝑦̇ , 𝜔𝐵3𝑧̇ )

𝛿𝑀3𝑧 =1

2𝐿𝑐𝑜𝑠𝜃3 ∙ 𝑑𝜃3 =

1

2𝐾3𝑐 ∙ 𝑑𝜃3

𝛿𝑁3 = (1

2(1

2𝐿𝑠𝑖𝑛𝜃3 ∙ 𝑑𝜃3 + 𝑑𝑥𝑝) ,

1

2(√3

2𝐿𝑠𝑖𝑛𝜃3 ∙ 𝑑𝜃3 + 𝑑𝑦𝑝) ,

1

2(𝐿𝑐𝑜𝑠𝜃3 ∙ 𝑑𝜃3 + 𝑑𝑧𝑝))

= (1

2(1

2𝐾3𝑠 ∙ 𝑑𝜃3 + 𝑑𝑥𝑝) ,

1

2(√3

2𝐾3𝑠 ∙ 𝑑𝜃3 + 𝑑𝑦𝑝) ,

1

2(𝐾3𝑐 ∙ 𝑑𝜃3 + 𝑑𝑧𝑝))

𝛿𝑁3𝑧 =1

2(𝐿𝑐𝑜𝑠𝜃3 ∙ 𝑑𝜃3 + 𝑑𝑧𝑝) =

1

2(𝐾3𝑐 ∙ 𝑑𝜃3 + 𝑑𝑧𝑝)

𝑊𝐴3𝐵3 +𝑊𝐵3𝐶3 = 𝑚𝐿𝑔 ∙1

2𝐾3𝑐 ∙ 𝑑𝜃3 + 𝐼𝐿𝜃3̈ ∙ 𝑑𝜃3 +𝑚𝑙𝑔 ∙

1

2(𝐾3𝑐 ∙ 𝑑𝜃3 + 𝑑𝑧𝑝)

+𝑚𝑙 [𝑎𝑁3𝑥 (1

2(1

2𝐾3𝑠 ∙ 𝑑𝜃3 + 𝑑𝑥𝑝)) + 𝑎𝑁3𝑦 (

1

2(√3

2𝐾3𝑠 ∙ 𝑑𝜃3 + 𝑑𝑦𝑝))

+ 𝑎𝑁3𝑧 (1

2(𝐾3𝑐 ∙ 𝑑𝜃3 + 𝑑𝑧𝑝))]

+ 𝐼𝑙 [𝜔𝐵3𝑥̇ (𝐻1(𝑑𝑧𝑝 − 𝐾3𝑐 ∙ 𝑑𝜃3) − 𝐻2 (𝑑𝑦𝑝 −√3

2𝐾3𝑠 ∙ 𝑑𝜃3))

+ 𝜔𝐵3𝑦̇ (𝐻2 (𝑑𝑥𝑝 −1

2𝐾3𝑠 ∙ 𝑑𝜃3) − 𝐻3(𝑑𝑧𝑝 − 𝐾3𝑐 ∙ 𝑑𝜃3))

+ 𝜔𝐵3𝑧̇ (𝐻3 (𝑑𝑦𝑝 −√3

2𝐾3𝑠 ∙ 𝑑𝜃3) − 𝐻1 (𝑑𝑥𝑝 −

1

2𝐾3𝑠 ∙ 𝑑𝜃3))]

= (𝑚𝐿𝑔 ∙1

2𝐾3𝑐 + 𝐼𝐿𝜃3̈ +𝑚𝑙𝑔 ∙

1

2𝐾3𝑐 +𝑚𝑙𝑎𝑁3𝑥 ∙

1

4𝐾3𝑠 +𝑚𝑙𝑎𝑁3𝑦 ∙

√3

4𝐾3𝑠 +𝑚𝑙𝑎𝑁3𝑧 ∙

1

2𝐾3𝑐

− 𝐼𝑙𝜔𝐵3𝑥̇ 𝐻1𝐾3𝑐 + 𝐼𝑙𝜔𝐵3𝑥̇ 𝐻2 ∙√3

2𝐾3𝑠 − 𝐼𝑙𝜔𝐵3𝑦̇ 𝐻2 ∙

1

2𝐾3𝑠 + 𝐼𝑙𝜔𝐵3𝑦̇ 𝐻3𝐾3𝑐

− 𝐼𝑙𝜔𝐵3𝑧̇ 𝐻3 ∙√3

2𝐾3𝑠 + 𝐼𝑙𝜔𝐵3𝑧̇ 𝐻1 ∙

1

2𝐾3𝑠)𝑑𝜃3

+ (𝑚𝑙𝑎𝑁3𝑥 ∙1

2+ 𝐼𝑙𝜔𝐵3𝑦̇ 𝐻2 − 𝐼𝑙𝜔𝐵3𝑧̇ 𝐻1) 𝑑𝑥𝑝

+ (𝑚𝑙𝑎𝑁3𝑦 ∙1

2− 𝐼𝑙𝜔𝐵3𝑥̇ 𝐻2 + 𝐼𝑙𝜔𝐵3𝑧̇ 𝐻3) 𝑑𝑦𝑝

+ (𝑚𝑙𝑎𝑁3𝑧 ∙1

2+ 𝐼𝑙𝜔𝐵3𝑥̇ 𝐻1 − 𝐼𝑙𝜔𝐵3𝑦̇ 𝐻3 +𝑚𝑙𝑔 ∙

1

2) 𝑑𝑧𝑝

= 𝑇𝐶𝑑𝜃3 + 𝑇31𝑑𝑥𝑝 + 𝑇32𝑑𝑦𝑝 + 𝑇33𝑑𝑧𝑝