Professional Documents
Culture Documents
Kinematic, Dynamic and Workspace Analysis of A Novel 6-Dof Parallel Manipulator
Kinematic, Dynamic and Workspace Analysis of A Novel 6-Dof Parallel Manipulator
by
Hrishi L. Shah
degree of
Master of Science
iii | P a g e
Acknowledgement
I express my sincerest gratitude for my advisor, Dr. Krovi, for the constant guidance and support
I also express my thanks to Dr. Roger Mayne and Dr. John Crassidis for being my committee members
I also thank the ARMLAB members for their support. Special thanks goes to LengFeng and Madu for
helping to write the publications related to this thesis. I would also like to thank Sumit for being an excellent
labmate, roommate and project partner and for his critique to my ideas and concepts throughout the duration of
the thesis. I am also grateful to Xiaobo and Colin for being helpful and patient whenever I needed their help.
I thank my friends in Buffalo, Vivek, Suresh, Vinay, for making this stay comfortable, enjoyable and
productive. I also thank Dr. and Mrs. Dhiraj Shah for helping continually and graciously during my entire stay in
Buffalo.
In the end, but most importantly, I would like to thank my parents, who have always supported my dreams
and motivated me to achieve them. I dedicate this work to their unending love and support, even when I was
iv | P a g e
Table of Contents
Acknowledgement ................................................................................................................................... iv
Table of Equations................................................................................................................................... xi
Abstract ..................................................................................................................................................... 1
1. Introduction ....................................................................................................................................... 3
3.1.2.3. Case 2: ‘cut’ at Point D: all joint Variables and Task Variables....................................... 15
3.1.2.4. Case 3: ‘cut’ at Point C: 6 joint Variables and Task Variables ......................................... 16
v|Page
3.3. Gauss Divergence Theorem ....................................................................................................... 19
3.4. Simulink/Simmechanics.............................................................................................................. 21
4. Workspace Analysis........................................................................................................................ 23
4.2.4. Recursive nearest neighbor search using Inverse Position Kinematics ............................... 36
vi | P a g e
5.2. Dynamic Analysis ....................................................................................................................... 47
9. Conclusion....................................................................................................................................... 64
vii | P a g e
11.2.2. Set of 6 joint variables and 3 task variables ..................................................................... 11-2
viii | P a g e
11.6.4. 6 joint variables + 6 task variables set ........................................................................... 11-14
Table of Figures
Figure 1 - Hexapod..................................................................................................................................... 4
Figure 6 - Constant Orientation Workspace of 3RPR (phi=0) using brute force method (dark black
patch) and Gauss divergence method (thick red solid line) .................................................... 21
Figure 7 - (a) Simple pendulum problem; (b) modeled in MapleSim environment; (c) generated EOMs,
plot of results from forward dynamic simulation, and visualization of the pendulum in
motion. .................................................................................................................................. 23
Figure 13 - Overlay of 3PRR individual workspaces with brute-force method workspace ......................... 29
Figure 18 - Hexapod workspace for [0 0 0] orientation using brute force method with inverse position
kinematics routine.................................................................................................................. 33
ix | P a g e
Figure 19 – Vector diagram for Geometry-based checks on Hexapod ...................................................... 34
Figure 22 - Hexapod workspace for [0 0 0] orientation using brute force method with geometric
checks ................................................................................................................................... 36
Figure 23 : Comparative study of all grid search methods (a) log-log graph of implementation time
w.r.t. grid size (b) log-log graph of % Volume error w.r.t. grid size ......................................... 37
Figure 24 : Vector loop for Exact workspace using geometric constraints ................................................ 38
Figure 26 : Comparison of workspace from grid-search method (Shaded) and Exact workspace
orientation.............................................................................................................................. 41
Figure 30 : [30 20 0] orientation workspace using Exact workspace method (MATLAB) ........................... 42
Figure 31 : 3D Workspace by Exact workspace CAD method- ([α, β, γ]T = [30, 20, 0]) ............................. 42
Figure 33 - Comparison of Stewart Gough platform results with Yoshito paper [15] ................................. 43
x|Page
Figure 42 - Hexapod Maplesim model ...................................................................................................... 59
calculated (b) Difference in prismatic velocities calculated (c) Actual prismatic positions
Table of Equations
Equation 3-3 : Velocity constraints in terms of dependent and independent variables .............................. 12
Equation 3-4 : Velocity relation between dependent and independent variables ....................................... 12
Equation 3-5 : Acceleration constraints between dependent and independent variables .......................... 13
Equation 3-18 : Dynamic terms related to EOM derivation for 3RPR ........................................................ 18
Equation 3-19 : Euler Lagrange formulation for part of EOM of 3RPR ...................................................... 18
Equation 3-22 : Gauss divergence theorem adaptation for area measurement (LHS) .............................. 19
Equation 4-1 : Basic Vector loop closure Equation for Hexapod ............................................................... 34
Equation 4-2 : rmax and rmin values for geometric checks method .............................................................. 35
Equation 4-3 : Vector loop closure Equation for Geometry constraints modeling of Hexapod ................... 38
Equation 4-4 : Rearranged vector loop closure equation for Geometry constraints modeling of
Hexapod ................................................................................................................................ 38
Equation 4-5 : Possible combinations of three sphere intersection points for Hexapod constant
Equation 5-11 : Dynamic matrices for the 3PRR (9 joint variables set) ..................................................... 48
Equation 11-1 : Area under circular arc using Gauss divergence theorem ............................................ 11-3
Equation 11-2 : Area under line using Gauss divergence theorem ........................................................ 11-3
xii | P a g e
Abstract
A parallel manipulator is a closed loop kinematic chain mechanism that is connected to the base via
multiple independent kinematic chains. Parallel manipulators have found numerous applications in
industries ranging from airplane simulators to high-speed pick and place robots due to their higher load-
carrying capacities and improved stiffness as compared to their serial counterparts. These advantages
arise due to the presence of kinematic-closed loops within the manipulator that allow for the load to be
transmitted to the ground via multiple chains. However, these kinematic closed loops also cause the
workspace of parallel manipulators to be severely limited and also pose a major challenge to their analysis
and control. Another problem is the evaluation of the workspace that is simultaneous affected by so many
Parallel platform manipulators consist of a central platform attached to the base via multiple legs. One
of the most popular parallel manipulators is the 6-DOF Stewart-Gough platform. It has 6-UPS architecture
and has been the focus of much work till date. However, the main problem is the power consumption of the
manipulator because of the bulky prismatic actuators lying along the links. Hence, we explore a novel
design of a parallel manipulator with 6PUS architecture that has the prismatic actuators attached to the
base, thereby making the legs lighter and the device energy efficient on the whole. We will refer to this
novel design as the “Hexapod” for the rest of this work. The goal of this work is to look into the following
• Workspace analysis
In this work, we dwell into the workspace analysis of parallel platform manipulators. Since the
workspace of a parallel manipulator is quite complex, it takes a considerable amount of time to compute it.
However, in order to optimize a parallel mechanism for workspace, an efficient and universal way of
workspace analysis is necessary. We propose an improved method of calculating the constant orientation
workspace of any parallel platform manipulator. Finally, we use a CAD package to speed up the process to
make a case for CAD-enhanced workspace analysis and showcase its general application to any parallel
platform manipulator.
1|Page
Another study explores a novel method for computing the equations of motion automatically by using a
symbolic computation engine called Maple. We also explore how this method of symbolic equation
generation, coupled with an easy to use interface called Maplesim; can be beneficial in augmenting the
learning in robotics courses. The next task is to employ these EOM’s to formulate control strategies and
2|Page
1. Introduction
A parallel platform manipulator (PPM) is a device whose end-effector is attached to the ground via
multiple serial chains that provides closed kinematic loops in the system for better load handling capacity
and stiffness. It is these qualities that make it applicable in a wide range of applications ranging from flight
simulators to high speed milling machines. However, it is these closed kinematic loops that increase the
An important advantage of parallel platform manipulator is that its superior structural rigidity renders it
as the better choice over serial chain manipulator (SCM) for moving heavy loads and high precision
machining tasks. The advantages include very high accuracy, better stiffness ratio, more payload capacity
Owing to their rolling and tilting capabilities and rigidity imparted by the closed loop coupled
architecture, the parallel platform robots serve well as moving platforms that perform tasks requiring
flexibility, precision and rigidity. Some of their end applications include tool base for multi-axis CNC
machining, flight simulator, simulation of wave induced motion on naval cargo ships, telescopes, fine
Despite the advantages that PPMs bring about in articulated mechanical activities, a down side
remains: the supporting links that form coupled closed loop kinematic chains add further constraints into the
Also, the high non-linearity generated in the loop closure equation for the Hexapod (Hex/H) platform
adds to the complexity of the problem as the link interference constraints cannot be fully detected
analytically. This situation creates a demand for an efficient numerical computational approach that also
incorporates the analytical techniques. Parallel robots are highly sensitive to dimensioning; hence accuracy
Finally, this thesis is intended as a learning ground for understanding large and complex articulated
multi-body systems (AMBS). The Hexapod, with its multiple parts coupled together by multiple joints that
can be categorized as 6 screw joints (SJ), 6 universal joints (UJ) and 3 spherical joints (SJ), is a complex
example illustrating the mechanics and control in a complex multi-body system. Also, the complexity of this
3|Page
system demands the use of different analytical techniques, numerical simulation tools, software
environments and computer-hardware interfaces, thus providing enough opportunities for practicing and
In this thesis work, the focus is placed on the study of a Novel 6 degree-of-freedom (DOF) PPM
referred to as a Hexapod.
Figure 1 - Hexapod
This device is capable of moving high payload at high acceleration within a compact workspace. The
platform is joined to rigid fixed length arms at three points that form an equilateral triangle. The arms are
linked to the platform in pairs, each pair coming together at one of their ends and subsequently attached to
the manipulator though a spherical joint. The free ends of each of the six arms are linked to the base by a
universal joint which can slide on the base. The bases of the arms are along prismatic joints. The same pair
of links that join together at the platform occupy one of the straight line axes, thus there are three straight
horizontal slots on the platform, each with two link bases sliding in it. The three slots are mutually aligned
on the rigid base such that they are collinear to each of three arms of an equilateral triangle.
4|Page
1.2. Goals of this work
The goals of this work relate to the following aspects of parallel manipulators:
• Workspace Analysis: Workspaces of parallel manipulators are quite complex in shape and hence,
not easily computed. Numerical techniques are generally slow for accurate results and inaccurate
when fast results are desired. We only focus on the constant orientation workspace of parallel
manipulators. We change the normal approach of an exhaustive grid search using inverse position
• Improve each computation by replacing inverse position kinematics with a geometric check
routine
• Eliminate grid search altogether by using a geometric constraint algorithm that finds the
• EOM generation for kinematic and dynamic analysis, simulation and model-based control : We
explore how the hand-derivation of complex EOMs for parallel manipulators can be simplified by
using a symbolic computation engine, Maple. We also examine an automatic block-based modeling
tool, Maplesim, built onto Maple and how this tool can not only further help alleviate many
drawbacks associated with hand-derivation but also eliminate the need for programmed symbolic
computation. Finally, we validate the EOM and forward dynamics simulation results against
• In an additional study, we also delve into the usefulness of Maplesim for augmenting learning in
5|Page
1.3. Model Simplification
Due to the 3D structure of the model and numerous joints, a dynamic analysis and control was difficult
from scratch. Hence, the model was simplified by taking a 2D projection from the top and side views.
In the top view, each pair of moving slider blocks (prismatic joints) was simplified to single blocks
sliding along the rails. The universal and prismatic joints were assumed as revolute joints. Thus this
simplification converts the three dimensional system to a two dimensional 3-PRR (Prismatic-Revolute-
6|Page
Revolute) manipulator. The resulting model is as shown. This system emulates three of the DOF of the
Hexapod, i.e. 2 displacements (x and y axes) and one rotation (about z axis).
In the side view, each pair of sliders and universal joints simplified to a revolute-prismatic joint. Hence,
a 3RPS (Revolute Prismatic Spherical) system resulted. This system emulates the remaining three DOF of
the Hexapod, i.e. one translation (z axis) and two rotations (about x and y axes).
At the end, we model the Hexapod and try to control it using kinematic and dynamic models.
We set the angle of the platform to [0,0,φ] and the position of the center of the platform to be [x,y,z0],
2
Z axis
0
3
-1 2 2.5
1 1.5
Y axis 0 0.5
-1 -0.5
-2 -1.5
-2.5 -2
X axis
7|Page
Figure 3 - Hexapod model Simplification to 3-PRR
From the above diagram, it is inferred that the variations in s1 and s2 have a one-to-one mapping with
the xlocal and ylocal values with fixed z=z0 and ay=az=0; which in turn are directly dependent on the [x, y, φ] of
the central platform (for 3PRR). Hence, we can say that in this scenario, the 3PRR is a quotient
In Section 2, we survey some of the work done on kinematic and dynamic modeling and control as well
as workspace analysis of PPMs. In Section 3, we explore all the mathematical and technological tools used
in this thesis. In section 4, we undertake the workspace analysis of the 3PRR and Hexapod and showcase
methods to make the process more efficient. In Sections 5 and 6, we see how the concepts of kinematic
and dynamic modeling and control are applied to a 3PRR and consecutively the Hexapod. In Section 7, we
conduct a review of the effort made to develop MapleSim, one of the technological tools, for augmenting
learning in basic robotics courses. Finally, we discuss the future scope and conclude.
8|Page
2. Literature Review
Many efforts have been made in the past to model the kinematics and dynamics of parallel
manipulators in order to build model based controllers that have been found to be much more stable than
Merlet [1], also presents an excellent treatise on parallel robots in his book on Parallel Robots. It
gradually introduces the reader to parallel robot through its historical development, basic architecture,
kinematic analysis, static and dynamic analysis, determination of singular configurations and workspace
A rather detailed and generic mathematical treatment of Parallel Manipulators has been discussed by
Merlet [2]. This paper presents a general mathematical framework to estimate the Jacobian, or more
precisely the inverse Jacobian matrices. It focuses on end-effector motions that cannot be detected by
One of the most common parallel platform manipulators, the Stewart-Gough platform, has been the
Xi and Sinatra [6], presented an analytical study of the inverse dynamics of hexapods with fixed-length
legs. The hexapod model used is a chain Prismatic-Universal-Spherical links connecting the base to the
platform through six legs. The inverse kinematic analysis including the Jacobian calculation has been done
using the loop-closure equations. The inverse dynamic calculations have been done through Newton-Euler
Madu Sathianarayan [7], attempted a vector analysis of the 6-PUS manipulator in his thesis and
showed that it is more energy efficient than the 6-UPS architecture of the Stewart Gough platform.
Another active area of research has been the simplification of a complex manipulator into smaller,
simpler quotient manipulators. Meng et. al. [8], explored the concept to set up a geometric theory for
analysis and synthesis of serial manipulator sub-chains given the desired motion type of a sub-6DOF
parallel manipulator.
9|Page
Many efforts have been made to compute the kinematic and dynamic models or planar parallel
manipulators like 3PRR, 3RPR etc. Staicu [9], calculated the inverse dynamics of the 3PRR mechanism
using the principle of virtual work. Staicu [10], also solved the inverse dynamics of the 3RPR mechanism
using the principle of virtual work followed by a power requirement analysis of two actuation schemes –
Several attempts in the past have succeeded in studying the workspace of 6-DOF parallel platform
manipulators. Some early attempts like Saxena et. al. [11] were able to describe the constant orientation
workspace using the parameter sweep method. Bonev and Gosselin in [12] proposed the geometric
approach to handling workspaces of parallel manipulators using a 6-R-U-S mechanism as a case study.
Thereafter, there has been considerable research concerning the constant orientation workspace of 6-DOF
PPMs but very few of them concentrated on symbolically solving and automating this procedure. An
attempt was made [13] to find the total and singularity free orientation workspaces (previously referred to as
constant position workspace) of the Stewart platform. Merlet et. al. [14], was able to find various subsets of
the orientation workspace. The constant position workspace also can be solved by using geometric
methods but the analysis was incomplete at the time the thesis was written.
Yoshito et. al. [15], attempted to use the intersection of multiple spheres describing the feasible
movements of all the different serial manipulators. However, the work remained limited to the visualization
case and no efforts at quantization were pursued i.e. the intersection volumes were never computed nor
were any optimization methods deployed. A similar intersection-workspace visualization concept was used
[16] a spatial 6-PRRS using CATIA programmatic modeling approach. In contrast, in this paper, we seek to
use the CAD package to both (i) characterize various aspects of the workspace using quantitative-
measures; as well as (ii) use workspace performance-measures for optimization to achieve another level of
In a more recent study by Ngoc et. al. [17], a 6-DOF parallel platform manipulator of Prismatic-
Spherical-Universal (PSU) sequence of joints has been simulated on a prototype simulator. Inverse position
analysis, Jacobian analysis and workspace analysis have been done before a successful single operator
10 | P a g e
ride. This prototype developed is somewhat similar to the Hexapod which is a Prismatic-Universal-Spherical
(PUS) 6-DOF-PM.
11 | P a g e
3. Mathematical and Technological Tools
The main difference between analysis of parallel and serial manipulators is the constraints relating
different states of the system. These constraints allow only some of the states of the system to be
independently controlled while the rest become dependent. The choice of dependent and independent
For a general parallel manipulator with n states and m degrees of freedom (or independent states),
So, the vector of n states, q, can be broken up into a vector with (n-m) dependent states, qdep and
There are many different formulations available for the same system, each depending upon the choice
of dependent and independent variables, which actually depends upon where the mechanism is ‘cut’ to
create the constraints. To explore these choices, a 3-RPR mechanism is considered as an example.
3.1.1. Framework
𝐶𝐶 = 0
On differentiating the position constraints, the velocity constraints in the system are obtained and can
be written as
𝐴𝐴 ∙ 𝑞𝑞̇ = 0
This equation can be decoupled into the independent and dependent states and can be rewritten as
Hence, the dependent states can be expressed as a function of the independent states as
12 | P a g e
On differentiating Equation 3-3, we get the acceleration level constraints
𝐴𝐴̇ ∙ 𝑞𝑞̇ + 𝐴𝐴 ∙ 𝑞𝑞̈ = 0 → 𝐴𝐴̇ ∙ 𝑞𝑞̇ + 𝐴𝐴𝑑𝑑𝑑𝑑𝑑𝑑 ∙ 𝑞𝑞̈ 𝑑𝑑𝑑𝑑𝑑𝑑 + 𝐴𝐴𝑖𝑖𝑖𝑖𝑖𝑖 ∙ 𝑞𝑞̈ 𝑖𝑖𝑖𝑖𝑖𝑖 = 0
Hence, the 𝑞𝑞̈ 𝑑𝑑𝑑𝑑𝑑𝑑 can again be expressed in terms of 𝑞𝑞̈ 𝑖𝑖𝑛𝑛𝑛𝑛 and 𝑞𝑞̇ .
3.1.2.1. Nomenclature
Ci : second revolute joint connecting to the platform for ith RPR subsystem
Differentiating,
𝑋𝑋𝑋𝑋̇𝑖𝑖 = {−(𝑙𝑙𝑖𝑖1 + 𝑠𝑠𝑖𝑖 ) ∗ sin(𝜃𝜃𝑖𝑖1 ) − 𝑙𝑙𝑖𝑖2 ∗ sin(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 )} ∗ 𝜃𝜃̇𝑖𝑖1 + 𝑠𝑠̇𝑖𝑖 ∗ 𝑐𝑐𝑐𝑐𝑐𝑐(𝜃𝜃𝑖𝑖1 ) − 𝑙𝑙𝑖𝑖2 ∗ sin(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 ) ∗ 𝜃𝜃̇𝑖𝑖2
𝑌𝑌𝑌𝑌̇𝑖𝑖 = {(𝑙𝑙𝑖𝑖1 + 𝑠𝑠𝑖𝑖 ) ∗ cos(𝜃𝜃𝑖𝑖1 ) + 𝑙𝑙𝑖𝑖2 ∗ cos(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 )} ∗ 𝜃𝜃̇𝑖𝑖1 + 𝑠𝑠̇𝑖𝑖 ∗ 𝑠𝑠𝑠𝑠𝑠𝑠(𝜃𝜃𝑖𝑖1 ) + 𝑙𝑙𝑖𝑖2 ∗ cos(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 ) ∗ 𝜃𝜃̇𝑖𝑖2
𝑋𝑋𝑋𝑋̇𝑖𝑖 −(𝑙𝑙𝑖𝑖1 + 𝑠𝑠𝑖𝑖 ) ∗ sin(𝜃𝜃𝑖𝑖1 ) − 𝑙𝑙𝑖𝑖2 ∗ sin(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 ) cos (𝜃𝜃𝑖𝑖1 ) −𝑙𝑙𝑖𝑖2 ∗ sin(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 ) 𝜃𝜃̇𝑖𝑖1
̇
�𝑌𝑌𝑌𝑌𝑖𝑖 � = � (𝑙𝑙𝑖𝑖1 + 𝑠𝑠𝑖𝑖 ) ∗ cos(𝜃𝜃𝑖𝑖1 ) + 𝑙𝑙𝑖𝑖2 ∗ cos(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 ) 𝑠𝑠𝑠𝑠𝑠𝑠(𝜃𝜃𝑖𝑖1 ) 𝑙𝑙𝑖𝑖2 ∗ cos(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 ) � ∙ � 𝑠𝑠̇𝑖𝑖 �
𝜃𝜃̇𝑖𝑖 1 0 1 𝜃𝜃̇𝑖𝑖2
States, n=9
𝑞𝑞̇ = [𝜃𝜃̇11 , 𝑠𝑠̇1 , 𝜃𝜃̇12 , 𝜃𝜃̇21 , 𝑠𝑠̇2 , 𝜃𝜃̇22 , 𝜃𝜃̇31 , 𝑠𝑠̇3 , 𝜃𝜃̇32 ]
𝑞𝑞̇ 𝑖𝑖𝑖𝑖𝑖𝑖 = �𝜃𝜃̇11 , 𝑠𝑠̇1 , 𝜃𝜃̇12 �𝑎𝑎𝑎𝑎𝑎𝑎 𝑞𝑞̇ 𝑑𝑑𝑑𝑑𝑑𝑑 = [𝜃𝜃̇21 , 𝑠𝑠̇2 , 𝜃𝜃̇22 , 𝜃𝜃̇31 , 𝑠𝑠̇3 , 𝜃𝜃̇32 ]
𝑋𝑋𝑋𝑋1 − 𝑋𝑋𝑋𝑋2
⎡ 𝑌𝑌𝑌𝑌 − 𝑌𝑌𝑌𝑌 ⎤
⎢ 1 2
⎥
𝜃𝜃1 − 𝜃𝜃2
𝐶𝐶 = ⎢ ⎥
⎢𝑋𝑋𝑋𝑋1 − 𝑋𝑋𝑋𝑋3 ⎥
⎢ 𝑌𝑌𝑌𝑌1 − 𝑌𝑌𝑌𝑌3 ⎥
⎣ 𝜃𝜃1 − 𝜃𝜃3 ⎦
𝑏𝑏𝑏𝑏 + (𝑙𝑙11 + 𝑠𝑠1 ) ∗ cos(𝜃𝜃11 ) + 𝑙𝑙12 ∗ cos(𝜃𝜃11 + 𝜃𝜃12 ) − 𝑏𝑏𝑏𝑏2 − (𝑙𝑙21 + 𝑠𝑠2 ) ∗ cos(𝜃𝜃21 ) − 𝑙𝑙22 ∗ cos(𝜃𝜃21 + 𝜃𝜃22 )
⎡ 1 ⎤
𝑏𝑏𝑏𝑏1 + (𝑙𝑙11 + 𝑠𝑠1 ) ∗ sin(𝜃𝜃11 ) + 𝑙𝑙12 ∗ sin(𝜃𝜃11 + 𝜃𝜃12 ) − 𝑏𝑏𝑏𝑏2 − (𝑙𝑙21 + 𝑠𝑠2 ) ∗ sin(𝜃𝜃21 ) − 𝑙𝑙22 ∗ sin(𝜃𝜃21 + 𝜃𝜃22 )
⎢ ⎥
𝜃𝜃11 + 𝜃𝜃12 − 𝜃𝜃21 − 𝜃𝜃22
𝐶𝐶 = ⎢ ⎥
⎢𝑏𝑏𝑏𝑏1 + (𝑙𝑙11 + 𝑠𝑠1 ) ∗ cos(𝜃𝜃11 ) + 𝑙𝑙12 ∗ cos(𝜃𝜃11 + 𝜃𝜃12 ) − 𝑏𝑏𝑏𝑏3 − (𝑙𝑙31 + 𝑠𝑠3 ) ∗ cos(𝜃𝜃31 ) − 𝑙𝑙32 ∗ cos(𝜃𝜃31 + 𝜃𝜃32 )⎥
⎢ 𝑏𝑏𝑏𝑏1 + (𝑙𝑙11 + 𝑠𝑠1 ) ∗ sin(𝜃𝜃11 ) + 𝑙𝑙12 ∗ sin(𝜃𝜃11 + 𝜃𝜃12 ) − 𝑏𝑏𝑏𝑏3 − (𝑙𝑙31 + 𝑠𝑠3 ) ∗ sin(𝜃𝜃31 ) − 𝑙𝑙32 ∗ sin(𝜃𝜃31 + 𝜃𝜃32 ) ⎥
⎣ 𝜃𝜃11 + 𝜃𝜃12 − 𝜃𝜃31 − 𝜃𝜃32 ⎦
14 | P a g e
Hence, differentiating the position constraint matrix C results in
𝑋𝑋𝑋𝑋̇ − 𝑋𝑋𝑋𝑋̇2
⎡ 1 ⎤
⎢ 𝑌𝑌𝑌𝑌̇1 − 𝑌𝑌𝑌𝑌̇2 ⎥
⎢ 𝜃𝜃̇ − 𝜃𝜃̇2 ⎥ 𝐽𝐽 ∙ 𝑞𝑞̇ − 𝐽𝐽2 ∙ 𝑞𝑞̇ 2 𝐽𝐽 −𝐽𝐽2 0
𝐶𝐶̇ = ⎢ 1 ⎥ =�1 1 �=�1 � ∙ 𝑞𝑞̇
̇
𝑋𝑋𝑋𝑋 − 𝑋𝑋𝑋𝑋3 ̇ 𝐽𝐽1 ∙ 𝑞𝑞̇ 1 − 𝐽𝐽3 ∙ 𝑞𝑞̇ 3 𝐽𝐽1 0 −𝐽𝐽3
⎢ 1 ⎥
̇ ̇
⎢ 𝑌𝑌𝑌𝑌1 − 𝑌𝑌𝑌𝑌3 ⎥
⎣ 𝜃𝜃̇1 − 𝜃𝜃̇3 ⎦
3.1.2.3. Case 2: ‘cut’ at Point D: all joint Variables and Task Variables
In this case, the only difference is that the task space variables are selected as independent and all
States, n=12
𝑞𝑞 = [𝑥𝑥𝑥𝑥, 𝑦𝑦𝑦𝑦, 𝜑𝜑𝜑𝜑, 𝜃𝜃11 , 𝑠𝑠1 , 𝜃𝜃12 , 𝜃𝜃21 , 𝑠𝑠2 , 𝜃𝜃22 , 𝜃𝜃31 , 𝑠𝑠3 , 𝜃𝜃32 ]
𝑞𝑞̇ = [𝑥𝑥𝑥𝑥̇, 𝑦𝑦𝑦𝑦̇, 𝜑𝜑𝜑𝜑̇, 𝜃𝜃̇11 , 𝑠𝑠̇1 , 𝜃𝜃̇12 , 𝜃𝜃̇21 , 𝑠𝑠̇2 , 𝜃𝜃̇22 , 𝜃𝜃̇31 , 𝑠𝑠̇3 , 𝜃𝜃̇32 ]
𝑞𝑞̇ 𝑖𝑖𝑖𝑖𝑖𝑖 = [𝑥𝑥𝑥𝑥̇, 𝑦𝑦𝑦𝑦̇, 𝜑𝜑𝜑𝜑̇]𝑎𝑎𝑎𝑎𝑎𝑎 𝑞𝑞̇ 𝑑𝑑𝑑𝑑𝑑𝑑 = [𝜃𝜃̇11 , 𝑠𝑠̇1 , 𝜃𝜃̇12 , 𝜃𝜃̇21 , 𝑠𝑠̇2 , 𝜃𝜃̇22 , 𝜃𝜃̇31 , 𝑠𝑠̇3 , 𝜃𝜃̇32 ]
𝑥𝑥𝑥𝑥 − 𝑋𝑋𝑋𝑋1
⎡ 𝑦𝑦𝑦𝑦 − 𝑌𝑌𝑌𝑌 ⎤
1
⎢ ⎥
5𝜋𝜋
⎢𝜑𝜑𝜑𝜑 + − 𝜃𝜃1 ⎥
⎢ 6 ⎥
⎢ 𝑥𝑥𝑥𝑥 − 𝑋𝑋𝑋𝑋2 ⎥
𝐶𝐶 = ⎢ 𝑦𝑦𝑦𝑦 − 𝑌𝑌𝑌𝑌2 ⎥
⎢ 𝜑𝜑𝜑𝜑 + 𝜋𝜋 − 𝜃𝜃 ⎥
2
⎢ 6 ⎥
⎢ 𝑥𝑥𝑥𝑥 − 𝑋𝑋𝑋𝑋3 ⎥
⎢ 𝑦𝑦𝑦𝑦 − 𝑌𝑌𝑌𝑌3 ⎥
⎢ 𝜋𝜋 ⎥
⎣ 𝜑𝜑𝜑𝜑 − − 𝜃𝜃3 ⎦
2
Equation 3-13 : 3RPR, Case 2, Position constraints
𝐼𝐼 −𝐽𝐽1 0 0
𝐴𝐴 = �𝐼𝐼 0 −𝐽𝐽2 0 �
𝐼𝐼 0 0 −𝐽𝐽3
15 | P a g e
q̇ 𝑑𝑑𝑑𝑑𝑑𝑑 = q̇ (4: 12), q̇ 𝑖𝑖𝑖𝑖𝑖𝑖 = q̇ (1: 3)
In this case, we express the points Ci in terms of x, y and φ in one representation and in terms of θi1
States, n=9
𝑞𝑞̇ = [𝑥𝑥𝑥𝑥̇, 𝑦𝑦𝑦𝑦̇, 𝜑𝜑𝜑𝜑̇, 𝜃𝜃̇11 , 𝑠𝑠̇1 , 𝜃𝜃̇21 , 𝑠𝑠̇2 , 𝜃𝜃̇31 , 𝑠𝑠̇3 ]
𝑞𝑞̇ 𝑖𝑖𝑖𝑖𝑖𝑖 = [𝑥𝑥𝑥𝑥̇, 𝑦𝑦𝑦𝑦̇, 𝜑𝜑𝜑𝜑̇]𝑎𝑎𝑎𝑎𝑎𝑎 𝑞𝑞̇ 𝑑𝑑𝑑𝑑𝑑𝑑 = [𝜃𝜃̇11 , 𝑠𝑠̇1 , 𝜃𝜃̇21 , 𝑠𝑠̇2 , 𝜃𝜃̇31 , 𝑠𝑠̇3 ]
The coordinates of point Ci can be expressed in terms of joint space variables (Ciq) as well as in terms
𝑙𝑙 𝑙𝑙 𝑙𝑙
⎡− ⎤
cos(𝜑𝜑) − sin(𝜑𝜑) 𝑥𝑥𝑥𝑥 2√3 2√3⎥
⎢ √3
[𝐶𝐶1𝑥𝑥 𝐶𝐶2𝑥𝑥 𝐶𝐶3𝑥𝑥 ] = � sin(𝜑𝜑) cos(𝜑𝜑) 𝑦𝑦𝑦𝑦� ∙ ⎢ 𝑙𝑙 𝑙𝑙 ⎥
0 0 1 ⎢ 0 2
− ⎥
2
⎣ 1 1 1 ⎦
On isolating the x and y coordinates and subtracting, we get a total of 6 constraints. The resulting
constraint matrix is
Where,
16 | P a g e
𝑙𝑙 𝑙𝑙 𝑙𝑙
⎡− ⎤
−sin(𝜑𝜑)𝜑𝜑̇ −cos(𝜑𝜑)𝜑𝜑̇ 𝑥𝑥𝑥𝑥̇
⎢ √3 2√3 2√3⎥
̇
[𝐶𝐶1𝑥𝑥 ̇
𝐶𝐶2𝑥𝑥 ̇ ] = � cos(𝜑𝜑)𝜑𝜑̇
𝐶𝐶3𝑥𝑥 −sin(𝜑𝜑)𝜑𝜑̇ 𝑦𝑦𝑦𝑦̇� ∙ ⎢ 𝑙𝑙 𝑙𝑙 ⎥
0 0 0 ⎢ 0 2
− ⎥
2
⎣ 1 1 1 ⎦
Dynamic modeling is generally done using the Euler Lagrange method of derivation. The main
Where,
t = time
Next, we use the equation stated above for each of the state variables and assemble the resulting nine
Where
M = Mass matrix
V = Force Vector
𝝉𝝉 = Torque input
λ = Constraint forces
In this case,
17 | P a g e
1 1 1 1 1 2
2
𝑇𝑇𝑖𝑖 = 𝑚𝑚𝑖𝑖0 �𝑥𝑥̇ 𝑖𝑖0 2
+ 𝑦𝑦̇ 𝑖𝑖0 2
� + 𝑚𝑚𝑖𝑖1 �𝑥𝑥̇ 𝑖𝑖1 2
+ 𝑦𝑦̇ 𝑖𝑖1 2
� + 𝑚𝑚𝑖𝑖2 �𝑥𝑥̇ 𝑖𝑖2 2
+ 𝑦𝑦̇ 𝑖𝑖2 � + 𝐽𝐽𝑖𝑖1 𝜃𝜃̇𝑖𝑖1
2
+ 𝐽𝐽𝑖𝑖2 �𝜃𝜃̇𝑖𝑖1 + 𝜃𝜃̇𝑖𝑖2 �
2 2 2 2 2
𝐿𝐿𝑖𝑖 = 𝑇𝑇𝑖𝑖
3
𝐿𝐿 = � 𝐿𝐿𝑖𝑖
𝑖𝑖=1
𝜋𝜋 = � 𝜏𝜏𝑖𝑖 ∙ θ̇i1
𝑖𝑖=1
Where,
To show the Euler-Lagrange formulation, only a part of the Lagrangian is shown so as to avoid long
2 ̇2
m10 l11 𝜃𝜃11 1
𝐿𝐿𝑝𝑝𝑝𝑝𝑝𝑝𝑝𝑝 = + 𝐽𝐽11 𝜃𝜃̇11
2
8 2
𝜕𝜕𝐿𝐿𝑝𝑝𝑝𝑝𝑝𝑝𝑝𝑝 2 ̇
m10 l11 𝜃𝜃11
= + 𝐽𝐽11 𝜃𝜃̇11
𝜕𝜕𝜃𝜃̇11 4
2 ̈
𝜕𝜕 𝜕𝜕𝐿𝐿𝑝𝑝𝑝𝑝𝑝𝑝𝑝𝑝 m10 l11 𝜃𝜃11
� �= + 𝐽𝐽11 𝜃𝜃̈11
𝜕𝜕𝜕𝜕 𝜕𝜕𝜃𝜃̇11 4
𝜕𝜕𝐿𝐿𝑝𝑝𝑝𝑝𝑝𝑝𝑝𝑝
=0
𝜕𝜕𝜃𝜃11
2 ̈
m10 l11 𝜃𝜃11
𝑂𝑂𝑂𝑂𝑂𝑂𝑂𝑂𝑂𝑂𝑂𝑂𝑂𝑂 𝑝𝑝𝑝𝑝𝑝𝑝𝑝𝑝 𝑒𝑒𝑒𝑒𝑒𝑒𝑒𝑒𝑒𝑒𝑒𝑒𝑒𝑒𝑒𝑒 ∶ + 𝐽𝐽11 𝜃𝜃̈11 = 𝜏𝜏1 ∙ 𝜃𝜃̇11
4
Equation 3-19 : Euler Lagrange formulation for part of EOM of 3RPR
18 | P a g e
3.3. Gauss Divergence Theorem
Most simply put, the Gauss divergence theorem states that the sum of all sources minus the sum of all
Formal statement: the outward flux of a vector field through a closed surface is equal to the volume
Mathematically: Let V be a region in space with boundary dV. Then the volume integral of the
divergence of F over V and the surface integral of F over the boundary dV of V are related by
The theorem can also be used for a planar area A bounded by a curve S, in which case the equation
simplifies to
�𝑑𝑑𝑑𝑑
� (∇. 𝐅𝐅)da = � 𝑭𝑭. 𝒏𝒏
𝑆𝑆 𝑑𝑑𝑑𝑑
For calculating the workspace volume using the Gauss divergence theorem, the problem statement is:
Given the curves/surfaces bounding the workspace, find the enclosed area/volume of the 2D/3D
workspace.
To do this, the function F is set so that ∇. 𝐅𝐅 = 𝟏𝟏 so that the left hand of Equation 3-21 simplifies to the
workspace area. Since the bounding curves of the workspace are known parametrically, the right side of
the equation is easily solved as shown below for the circular arc and the line. For any curve, we select
Equation 3-22 : Gauss divergence theorem adaptation for area measurement (LHS)
The derivations for the equations of area under circular and linear curves using the Gauss divergence
19 | P a g e
3.3.2.1. Example - 3-RPR mechanism
We seek to find the constant orientation workspace of the 3-RPR, i.e., the area reachable by the end-
The method for finding the bounding curves for this constant orientation workspace area is discussed
in 4.1.4. Following this method of geometrical constraints, we find the following series of curves bounding
30
20
10
-10
-20
-30
The 3-RPR workspace is simply the intersection of the three individual workspaces whose inner and
outer circles are represented as dark and thin dotted lines. The thick solid lines (red), represent the
boundaries of the intersection area(s) of the individual workspaces. The area of the bounded workspace is
calculated by cycling through all the bounding curves while applying Equation 11-1 to each one. In this
case, the computed area of 22.6035 units2 is reasonably close to the computed area of 22.54 units2
obtained using a brute force method with inverse position kinematics at each point. This is justified further
20 | P a g e
30
20
10
-10
-20
-30
Figure 6 - Constant Orientation Workspace of 3RPR (phi=0) using brute force method (dark black patch) and Gauss divergence
method (thick red solid line)
3.4. Simulink/Simmechanics
Simmechanics is a toolbox with Matlab that allows block diagram modeling and simulation of rigid body
systems. Instead of deriving and programming equations, one can use this multi-body simulation tool to
build a model composed of bodies, joints, constraints, and force elements that reflects the structure of the
system. An automatically generated 3-D animation helps visualize the system dynamics. Models complete
with mass, inertia, constraint, and 3-D geometry can be automatically imported from several CAD systems.
3.5. MapleSim
Maple is a computation software with easy and efficient symbolic computation built into it. It is used to
programmatically create symbolic equations of motion for several mechanisms during this thesis. Maplesim
is a toolbox built on top of Maple programming interface to allow for block diagram modeling and simulation
of electrical, hydraulic, rigid multi-body systems etc. The added advantage is that one can easily extract the
underlying equations of motion from the model by attaching a Maple sheet to the model. It has been mainly
21 | P a g e
used to validate the equations of motion generated by hand as well as validation of forward dynamic
simulation results.
(1) Idealize the problem by making appropriate assumptions, such as its inertia is like that of an
idealized rod.
(3) Develop the appropriate governing equation of motions (EOM) using Newton’s laws of motion
In the MapleSim approach, the simplified model is converted into a block based model as shown in
Figure 7(b). The model is then simulated in order to visualize its motion and extract the time history of
important parameters. Finally, the EOM are extracted by attaching a Maple sheet to the model and
exporting the dynamic matrices to Matlab for further analysis and simulation.
(a) (b)
22 | P a g e
(c)
Figure 7 - (a) Simple pendulum problem; (b) modeled in MapleSim environment; (c) generated EOMs, plot of results from forward
dynamic simulation, and visualization of the pendulum in motion.
4. Workspace Analysis
The general method for workspace computation is a brute-force search using an inverse position
kinematics routine at each point. However, this approach is inefficient because it leads to lost workspace if
the initial grid selection does not cover the entire workspace and lost time and efficiency if the initial grid is
fairly larger than the workspace. Furthermore, it does not allow much insight into the actual constraints and
working of the hexapod. This method can be improved upon in the following ways:
1. Improve the efficiency of a single computation by replacing inverse position kinematics with Boolean
geometric checks
2. Decrease the number computations by using a different search method (like the recursive nearest
neighbor-search method)
3. Devise a geometric method of workspace computation that eliminates the grid-based approach
23 | P a g e
4.1. 3PRR
4.1.1. Nomenclature
As discussed, this model is a simplified version of the Hexapod. Its detailed schematics are as shown.
Bi denote the vertices of the fixed equi-triangular base on which the prismatic joints Ai slide. αi denote
the inclinations of the three sides of the base with the inertial reference frame. Ci are the vertices of the
moving platform which is also assumed to be an equilateral triangle. AiCi are the fixed length links which
join the moving prismatic joints to the platform through two revolute joints. The origin of the absolute
reference frame is assumed to be fixed at O, the centroid of B1B2B3. The origin of a moving reference frame
is attached to point P, the centroid of C1C2C3. The references for the three prismatic joints are at the
centers of the three sides of the equi-triangular base. Φ is the angle of the central platform as expressed in
The joint variables are the prismatic joint positions (si) and revolute positions (θi1 and θi2) for all three
links (i = 1, 2, 3). The centroid of the central platform, which is also the endpoint of all the three individual
𝐿𝐿
𝐵𝐵1 𝑂𝑂 = 𝐵𝐵2 𝑂𝑂 = 𝐵𝐵3 𝑂𝑂 =
√3
𝑙𝑙
𝐶𝐶1 𝑃𝑃 = 𝐶𝐶2 𝑃𝑃 = 𝐶𝐶3 𝑃𝑃 =
√3
X = [xe,ye,φ]
In this method, we form a surface grid by taking an initial guess about a surface that might encompass
the entire workspace. The extent of this guess as well as the grid size are very important as they directly
affect the number of points created in the grid and hence the number of times the inverse position
kinematics routine is run. For best results, we iteratively form this initial guess to be fairly close to the actual
workspace so that the computation is complete in minimum time. For each inverse position kinematics
routine: (i) the three end points of the platform are calculated from the position of its centroid (a point on
the grid) and the constant orientation supplied; (ii) Each point is converted to the local reference frame and
the prismatic displacements are solved for by using the link lengths; (iii) For each prismatic slider, they are
checked for the prismatic joint limits (s < Lmax and s > Lmin). If this constraint is satisfied for all the 3 limbs,
As a benchmark, we included the brute-force grid-based search to obtain a baseline. Although more
intelligent methods are available, the order of magnitude improvement achieved by exploiting the
underlying structure/geometry into the computation is the aspect we are seeking to highlight.
25 | P a g e
The brute-force search using inverse position kinematics routine gives the following results:
7.5
6.5
5.5
4.5
3.5
2.5
-3 -2 -1 0 1 2 3
In the above figure, the black dotted lines represent the lines of action of the prismatic joints.
In this approach, we change the way the grid is sampled to get the workspace. The main algorithm is
shown on the side. We choose an initial feasible point by choosing random points until a feasible point is
encountered. Next, we choose all the neighbors of this point and check for feasibility. All feasible neighbors
form the parent set for the next iteration. A check is also incorporated to avoid checking points already
26 | P a g e
Figure 10 : Recursive nearest neighbor search algorithm
Although the improvements in the grid based method provide much faster results, any method with
grid-based searching can only be as accurate as the grid-spacing. Hence, we employ a method of
intersecting areas to find the workspace edges parametrically and then employ gauss divergence theorem
This method simply uses the commutative property of vectors to convert the constant orientation
workspace of any parallel platform manipulator into a problem of intersection of displaced individual
workspaces.
We begin with the vector equation for the end point of the mechanism, i.e., the center of the platform.
In case of a constant orientation workspace problem, the vector CiP remains constant throughout the
analysis. Also, the range of vector AiBi is known to be the range of the prismatic joint. Furthermore, the
27 | P a g e
length of vector BiCi is known to be constant. Hence, we can find a sweep of all the possible positions of
the vector AiCi in the form of a rectangle bounded on two sides by two semicircles. Thus, by knowing a
constant orientation, we can find all possible locations for the points Ci. This is illustrated in the figure
below.
B
2
2
C
1 3
A3 A2
0 C P C
B 1 2
3
-1
A B
1 1
-2
-3
-5 -4 -3 -2 -1 0 1 2 3 4 5
In this figure, the thick dotted lines represent the prismatic joint motion limits and the thin solid lines
28 | P a g e
Next, we just add the vector CiP beforehand in all the PRR linkages to shift the workspaces by certain
10
0
-6 -4 -2 0 2 4 6
Thus, by intersecting these 3 shifted areas, we can find the constant orientation workspace for the
3PRR. This fact can be verified by overlaying the areas with the workspace obtained using the brute-force
method.
10
0
-8 -6 -4 -2 0 2 4 6 8 10
using brute-force method and it matches reasonably with the common area between the three individual
workspaces.
The intersection can then be programmed for the Gauss-divergence theorem as discussed in 3.3.2.
10
0
-6 -4 -2 0 2 4 6
The workspace area as calculated by this method is 13.4748 units2, which is in close agreement with
the brute-force calculated volume of 13.4350 units2 (grid size=0.05) and 13.4792 units2 (grid size=0.01).
In order to improve the efficiency further, we can program a CAD package to compute the above stated
intersection automatically given the important geometric parameters. Since the workspace for a particular
orientation is 2D, we can use the third dimension, z, to represent the orientation angle φ and find the
The first step is to create the curves representing the changing direction of vector CiP as φ=z goes
from 0 to 2π. The next step is to sweep the areas created for the vectors AiCi over these curves and keep
30 | P a g e
Figure 15 - 3PRR constant orientation workspace for all orientations
The top surface of the solid is the constant orientation workspace for φ=0 and can be compared
against the figure above. The workspace can be now cut at any given value of z= φ to give the constant
orientation workspace at that point and the surface area can also be easily measured.
4.2. Hexapod
Since the Hexapod has 6 DOF spatial manipulator, visualizing its complete 6-D workspace would be
difficult. To overcome this issue, such workspaces are classified into various groups [18] out of which two
types are commonly considered important and studied: (i) a constant orientation workspace is the 3D space
of points where the manipulator can reach while keeping its orientation fixed; (ii) a constant position
workspace is a set of orientations possible for the manipulator while keeping the position of the platform
4.2.1. Nomenclature
31 | P a g e
P C
A2
A3 O
B
A1
The origin, O, is fixed at the centroid of the equilateral triangle (shown in think solid line) formed by the
axes of action of the 6 prismatic joints (two prismatic joints in opposite directions along each axis). In the
figure, Ai‘s represent the starting points of the prismatic joints whose axes are aligned with the x-axis of the
reference frame at Ai. The point B represents the actual position of the prismatic slider as well as the
universal joint mounted atop the slider. The point C represents the endpoint of the central platform (shown
in gray patch) as well as the spherical joint connecting the central platform with the 6 legs, BC (shown in
P C
O B
A1
We choose a Hexapod with the following parameters for the workspace evaluation:
32 | P a g e
4.2.2. Parametric Sweeps with Inverse Position Kinematics
In this method, we form a volume grid by taking an initial guess about a volume that might encompass
the entire workspace. The extent of this guess as well as the grid size are very important as they directly
affect the number of points created in the grid and hence the number of times the inverse position
kinematics routine is run. For best results, we iteratively form this initial guess to be fairly close to the actual
workspace so that the computation is complete in minimum time. For each inverse position kinematics
routine: (i) the three end points of the platform are calculated from the position of its centroid (a point on
the grid) and the constant orientation supplied; (ii) Each point is converted to the local reference frame and
the prismatic displacements are solved for by using the link lengths; (iii) For each prismatic slider, they are
checked for the prismatic joint limits (s < Lmax and s > Lmin). If this constraint is satisfied for all the 6 limbs,
The brute-force search using inverse position kinematics routine gives the following results:
Figure 18 - Hexapod workspace for [0 0 0] orientation using brute force method with inverse position kinematics routine
The surfaces are obtained by finding the minimum and maximum feasible z positions for each [x y]
33 | P a g e
4.2.3. Parametric Sweeps with Geometric checks
We can also use geometric checks to accelerate the workspace determination process to improve on
overall computation time for the grid-search method. To this end, the loop closure equation for each limb of
the platform can be written to formulate the position level constraints as:
�����⃗
𝑂𝑂𝑂𝑂 + �����⃗
𝑃𝑃𝑃𝑃 = �����⃗
𝑂𝑂𝑂𝑂 + �����⃗
𝐴𝐴𝐴𝐴
constraints. For the purpose of this analysis, we assume that all the three pairs of legs have lengths l1, l2
and l3 but the legs in the same subsystem (as in Figure 17 for TLS) are equal in length. This is basically
enforced to eliminate any singular configurations in the hexapod resulting from any of the limbs becoming
perpendicular to their corresponding prismatic axes. Secondly, this assumption simplifies the kinematic
problem.
Our main aim here is to find maximum and minimum values for the magnitude of vector AC so that we
can determine the feasibility of a point by checking if it is within the calculated range for all the three
subsystems. Considering the ith subsystem (i=1, 2, 3), the maximum value for AC will be achieved when the
34 | P a g e
minimum prismatic displacement is reached and the minimum value for AC will be reached when the
maximum prismatic displacement is reached. Note that xlocal is the x-coordinate of C expressed in the local
reference frame, Ai of the TLS in consideration. This is shown in Figure 20 and Figure 21.
From the figures, we can directly infer that the values for maximum (rmax) and minimum (rmin) lengths for
AC are:
2 2
𝑟𝑟𝑚𝑚𝑚𝑚𝑚𝑚 = 𝑙𝑙12 − (𝐿𝐿𝑚𝑚𝑚𝑚𝑚𝑚 − 𝑥𝑥𝑙𝑙𝑙𝑙𝑙𝑙𝑙𝑙𝑙𝑙 )2 + 𝑥𝑥𝑙𝑙𝑙𝑙𝑙𝑙𝑙𝑙𝑙𝑙
2
2
𝑟𝑟𝑚𝑚𝑚𝑚𝑚𝑚 = 𝑙𝑙12 − (𝐿𝐿𝑚𝑚𝑚𝑚𝑚𝑚 + 𝑥𝑥𝑙𝑙𝑙𝑙𝑙𝑙𝑙𝑙𝑙𝑙 )2 + 𝑥𝑥𝑙𝑙𝑙𝑙𝑙𝑙𝑙𝑙𝑙𝑙
Equation 4-2 : rmax and rmin values for geometric checks method
By checking if the actual distance from each local reference frame is between these values, we can
35 | P a g e
Figure 22 - Hexapod workspace for [0 0 0] orientation using brute force method with geometric checks
Noting that the number of feasible points in this case is the same as that in the previous case and on
comparison with the shape in Figure 19, we can infer that the geometric check is identical to the inverse
As suggested in method 2, we can change the method for the sweep from parametric to a more
efficient method. One such example is the recursive nearest neighbor-search. We choose random points
from the workspace till a feasible point is reached and then “explode” outwards from that point in a tree-
Noting that the number of points is equal to the previous two methods, we can infer that the method
gives accurate results. This method is found to be better than method 1 but could be improved by replacing
This method takes both suggestions 1 and 2 into consideration for achieving the best results among
It is found to be better than methods 2 and 3. However, the computation time for all the methods
36 | P a g e
Now, in order to perform a comparative study between the different methods, we use the common
computing platform with the following specification—Pentium dual-core 2.1Ghz processor with 4GB RAM
and Windows 7 Home Premium. The corresponding CPU times required to complete the workspace
computation are documented in Table 2. Note that this performance study is only intended to give a rough
Tree_GeomCheck -2
3
-2.5
2
-3
1
-3.5
0
-4 bruteF_InvPosKin
bruteF_GeomCheck
-1 -4.5 Tree_InvPosKin
Tree_GeomCheck
-2 -5
-2.5 -2 -1.5 -1 -2.5 -2 -1.5 -1
log10(Grid size) log10(Grid size)
(a) (b)
Figure 23 : Comparative study of all grid search methods (a) log-log graph of implementation time w.r.t. grid size (b) log-log graph
of % Volume error w.r.t. grid size
The following table outlines the comparative efficiencies of all grid-search methods.
TABLE 2 : COMPARATIVE EFFICIENCIES OF ALL GRID SEARCH METHODS FOR CONSTANT ORIENTATION WORKSPACE OF HEXAPOD AT GRID SIZE =0.0625
37 | P a g e
4.2.6. Exact Workspace determination using Geometric constraints
As in the earlier case, we begin with the geometric-constraints derived from the loop-closure equations.
�����⃗ + 𝑃𝑃𝑃𝑃
𝑂𝑂𝑂𝑂 �����⃗ = 𝑂𝑂𝑂𝑂
�����⃗ + 𝐴𝐴𝐴𝐴
�����⃗ + 𝐵𝐵𝐵𝐵
�����⃗
Equation 4-3 : Vector loop closure Equation for Geometry constraints modeling of Hexapod
For the forward position kinematics problem, given values for the actuated prismatic joints, OA and
PC are known and hence Equation 4-3 can be rewritten as:
�����⃗
𝑂𝑂𝑂𝑂 = �����⃗
𝑂𝑂𝑂𝑂 + �����⃗
𝐴𝐴𝐴𝐴 + �����⃗
𝐵𝐵𝐵𝐵 − �����⃗
𝑃𝑃𝑃𝑃
Equation 4-4 : Rearranged vector loop closure equation for Geometry constraints modeling of Hexapod
As can be seen, all the vectors except B1C1 and B2C2 are known and can be substituted for. Also, the
magnitude of the unknown vectors is known (link lengths). Hence, we can reduce the problem of finding the
end-effector position to a problem of intersecting spheres. This is illustrated graphically in Figure 24 which
In Figure 24, the dots along the line parallel to a prismatic joint axis through Ai represent the prismatic
joint limits (vectors shifted due to change in order of vector addition). Hence, according to Figure 24, the
link is at the maximum displacement of the right prismatic joint. Hence, the workspace can now be
calculated by cycling through the range of prismatic motions. We notice here that in order to any point
along the workspace boundaries, at least one of the prismatic joints must have reached their motion limits.
Hence, any surface in the entire workspace should lie along one of the limit spheres with a prismatic limit
point as its center and the link length as its radius. By the same token, any edge of the workspace
38 | P a g e
(intersection of two spherical surfaces) would be along the intersection of two spheres such that limit
constraints on any two prismatic sliders are active simultaneously. Similarly, any intersection point of the
workspace (intersection of two curves) would be the intersection of three spheres, or at those points, the
To find the workspace, we start with the intersection points between different spherical surfaces. We
note that there are a total of 12 limit points for the 6 prismatic joints. Considering that any prismatic joint can
only be at one of the two limits at the same instant, we eliminate two of the 12 initially available limit points
by assuming one prismatic joint at its limit. Thus, we find the total number of 3-sphere intersection points
possible by finding the total number of combinations possible from the formula
Equation 4-5 : Possible combinations of three sphere intersection points for Hexapod constant orientation workspace
Next, we find the actual intersection points for all of these possibilities and check whether they
satisfy conditions for the other links to reach the point in consideration (whether the other 3 links assumed
not to be at the limits are feasibly placed to reach this point). This criterion’s implementation is similar to the
previous method. This decreases the number of points that we need to find from 960 to the actual number
of points on the workspace (18 in this particular example). The last step is to find the points with 2 common
spheres and draw curves between them along the intersection of the two common spheres. Finally, we find
the curves having a common sphere index and plot surfaces between them along the common sphere. The
39 | P a g e
Figure 26 : Comparison of workspace from grid-search method (Shaded) and Exact workspace method (Solid line)
To verify the validity of this approach, we eliminate the surface plots from Exact workspace method
and overlay the resulting line plot with the workspace plot from grid Search Method as shown in Figure 26.
As expected, the workspace envelope obtained by Exact workspace method is similar to, though more
accurate than the one obtained from the parameter sweep method. Furthermore, it can also be verified that
the differences are less than the grid size used in grid Search method (0.05 in this case) as shown in Table
3. We see that as the grid size decreases, the min/max X/Y/Z values from the grid-search method converge
Exact
Grid search method
workspace Difference
(for different grid sizes)
method
However, we notice that the above three approaches relied on the user’s ability to code part of the
computation in Matlab. An alternative, more graphical approach is to intersect the three separate feasible
regions of all three TLS’ and form a workspace, which is possible using a 3D CAD package like Pro-E or
Solidworks. It should be noted that the CAD package is being used to compute the intersection of
the spheres and not to perform the 3D detailed drawing of the parallel manipulator in question!
40 | P a g e
Figure 27 - 3D [0 0 0] orientation workspace by Exact Workspace method
As further validation of the method, the 3D constant orientation workspace of the Hexapod is shown for
P
C
A2
A3 O
B
A1
41 | P a g e
Figure 30 : [30 20 0] orientation workspace using Exact workspace method (MATLAB)
Figure 31 : 3D Workspace by Exact workspace CAD method- ([α, β, γ] = [30, 20, 0])
T
The advantages of developing an efficient CAD based system for constant orientation workspace
computation can be seen for doing parametric studies and optimization. For example, if only limited space
is available for placing the entire system, it places a limit on the base triangle length (L). Hence, a
parametric study to maximize the workspace volume for a constant orientation of [0 0 0] needs to be
conducted by varying the leg lengths and central platform size (characterized by l). We can easily do this
computation in ProE by using the multi-objective design optimization tool. Our study had the following
parameter variations:
42 | P a g e
The program required approximately 42.5 seconds for a test run of 441 points (details shown in Table
4) thereby leading to a computation time of 0.0963 seconds/run, which is an order of magnitude better than
the best grid-search result for the same grid size. The results of this sweep were exported to a csv file and
0.5
0
1
2 4 5
3 2 3
This method can be applied to the Stewart Platform with minor changes to the CAD model.
Figure 33 - Comparison of Stewart Gough platform results with Yoshito paper [15]
We just need to change the equations for CP and change the local reference frame locations to suit
that for the Stewart platform. Doing these minor changes, one can easily get the workspace for a Stewart
platform. To demonstrate this concept, we compare against Yoshito's results for the home orientation, [0, 0,
0]T using the parameters from that paper as shown in Figure 33.
43 | P a g e
5. Kinematic and Dynamic Analysis of 3-PRR
θi1 : Angle of rotation of first revolute joint in ith PRR Sub-System with respect to prismatic joint axis
θi2 : Angle of rotation of second revolute joint in ith PRR Sub-System with respect to first revolute joint
We attempt to find the locations of the centers of mass of each body and also the endpoint of each link.
𝑙𝑙𝑖𝑖1
𝑥𝑥𝑖𝑖1 = 𝑏𝑏𝑥𝑥𝑥𝑥 + 𝑠𝑠𝑖𝑖 ∙ cos(𝛼𝛼𝑖𝑖 ) + ∙ cos(𝜃𝜃𝑖𝑖1 + 𝛼𝛼𝑖𝑖 )
2
𝑙𝑙𝑖𝑖1
𝑦𝑦𝑖𝑖1 = 𝑏𝑏𝑦𝑦𝑦𝑦 + 𝑠𝑠𝑖𝑖 ∙ 𝑠𝑠𝑠𝑠𝑠𝑠(𝛼𝛼𝑖𝑖 ) + ∙ sin(𝜃𝜃𝑖𝑖1 + 𝛼𝛼𝑖𝑖 )
2
𝑙𝑙𝑖𝑖2
𝑥𝑥𝑖𝑖2 = 𝑏𝑏𝑥𝑥𝑥𝑥 + 𝑠𝑠𝑖𝑖 ∙ cos(𝛼𝛼𝑖𝑖 ) + 𝑙𝑙𝑖𝑖1 ∙ cos(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1 ) + ∙ cos(𝛼𝛼𝑖𝑖 +𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 )
2
𝑙𝑙𝑖𝑖2
𝑦𝑦𝑖𝑖2 = 𝑏𝑏𝑦𝑦𝑦𝑦 + 𝑠𝑠𝑖𝑖 ∙ sin(𝛼𝛼𝑖𝑖 ) + 𝑙𝑙𝑖𝑖1 ∙ sin(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1 ) + ∙ sin(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 )
2
Equation 5-1 : 3PRR – Position of link masses
Center of platform:
𝑦𝑦𝑖𝑖3 = 𝑏𝑏𝑦𝑦𝑦𝑦 + 𝑠𝑠𝑖𝑖 ∙ 𝑠𝑠𝑠𝑠𝑠𝑠(𝛼𝛼𝑖𝑖 ) + 𝑙𝑙𝑖𝑖1 ∙ sin(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1 ) + 𝑙𝑙𝑖𝑖2 ∙ sin(𝛼𝛼𝑖𝑖 +𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 )
44 | P a g e
Now, differentiating the position level equations, we get
𝑙𝑙𝑖𝑖1
𝑥𝑥𝑖𝑖1̇ = 𝑠𝑠𝑖𝑖̇ ∙ cos(𝛼𝛼𝑖𝑖 ) − ∙ sin(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1 ) ∙ 𝜃𝜃𝑖𝑖1̇
2
𝑙𝑙𝑖𝑖1
𝑦𝑦𝑖𝑖1̇ = 𝑠𝑠𝑖𝑖̇ ∙ sin(𝛼𝛼𝑖𝑖 ) + ∙ cos(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1 ) ∙ 𝜃𝜃𝑖𝑖1̇
2
𝑙𝑙𝑖𝑖2
𝑥𝑥𝑖𝑖2̇ = 𝑠𝑠𝑖𝑖̇ ∙ cos(𝛼𝛼𝑖𝑖 ) − 𝑙𝑙𝑖𝑖1 ∙ sin(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1 ) ∙ 𝜃𝜃𝑖𝑖1̇ − ∙ sin(𝛼𝛼𝑖𝑖 +𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 ) ∙ (𝜃𝜃𝑖𝑖1̇ + 𝜃𝜃𝑖𝑖2̇ )
2
𝑙𝑙𝑖𝑖2
𝑦𝑦𝑖𝑖2̇ = 𝑠𝑠𝑖𝑖̇ ∙ sin(𝛼𝛼𝑖𝑖 ) + 𝑙𝑙𝑖𝑖1 ∙ cos(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1 ) ∙ 𝜃𝜃𝑖𝑖1̇ + ∙ cos(𝛼𝛼𝑖𝑖 +𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 ) ∙ (𝜃𝜃𝑖𝑖1̇ + 𝜃𝜃𝑖𝑖2̇ )
2
𝑥𝑥𝑖𝑖3̇ = 𝑠𝑠𝑖𝑖̇ ∙ cos(𝛼𝛼𝑖𝑖 ) − 𝑙𝑙𝑖𝑖1 ∙ sin(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1 ) ∙ 𝜃𝜃𝑖𝑖1̇ − 𝑙𝑙𝑖𝑖2 ∙ sin(𝛼𝛼𝑖𝑖 +𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 ) ∙ (𝜃𝜃𝑖𝑖1̇ + 𝜃𝜃𝑖𝑖2̇ )
𝑦𝑦𝑖𝑖3̇ = 𝑠𝑠𝑖𝑖̇ ∙ sin(𝛼𝛼𝑖𝑖 ) + 𝑙𝑙𝑖𝑖1 ∙ cos(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1 ) ∙ 𝜃𝜃𝑖𝑖1̇ + 𝑙𝑙𝑖𝑖2 ∙ cos(𝛼𝛼𝑖𝑖 +𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 ) ∙ (𝜃𝜃𝑖𝑖1̇ + 𝜃𝜃𝑖𝑖2̇ )
2𝜋𝜋 𝜋𝜋
𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 = (𝑖𝑖 − 1) ∙ + + 𝜑𝜑
3 6
From the above equations, we have 3 task space DOF’s and 9 joint space DOF’s. Therefore, we can
select 3 independent DOF’s from joint space. Since the prismatic joints are actuated, we select DOF’s 1, 4
45 | P a g e
In order to express the dependent DOF’s in terms of independent DOF’s, we rewrite the above
𝐽𝐽1 q̇ 1 − 𝐽𝐽2 q̇ 2 0
� � =� �
𝐽𝐽1 𝑞𝑞1 ̇ − 𝐽𝐽3 𝑞𝑞3̇ 6∗1 0 6∗1
𝑞𝑞1̇
𝐽𝐽1 −𝐽𝐽2 0 0
� � �𝑞𝑞2̇ � = � �
𝐽𝐽1 0 −𝐽𝐽3 6∗9 0 6∗1
𝑞𝑞3̇ 9∗1
Now, we have a matrix representation wherein each column in the J matrix represents the contribution
of a particular state in the system. Hence, we isolate the 1st, 4th and 7th columns corresponding to the
𝜃𝜃 ̇
⎡ 11 ⎤
⎢𝜃𝜃12̇ ⎥
𝑠𝑠1̇
⎢𝜃𝜃 ̇ ⎥ 0
𝐴𝐴𝑑𝑑𝑑𝑑𝑑𝑑 ∙ ⎢ 21 ⎥ + 𝐴𝐴𝑖𝑖𝑖𝑖𝑖𝑖 ∙ �𝑠𝑠̇2 � = � �
𝜃𝜃 ̇ 0 6∗1
⎢ 22 ⎥ 𝑠𝑠3̇
̇
⎢𝜃𝜃31 ⎥
⎣𝜃𝜃32̇ ⎦
The final step is to compute the independent DOF’s in terms of the required end-effectors DOF’s. In
The inversion of the matrix is using a pseudo inverse since the matrix is not square.
While calculating the, 𝑞𝑞̇ 𝑖𝑖𝑖𝑖𝑖𝑖 , we can also add a null-space component to accomplish secondary tasks.
Hence, given any state of the system, we can find the state derivatives.
46 | P a g e
5.2. Dynamic Analysis
We attempt the dynamic modeling using the Euler Lagrange method discussed in section 3.2.
Using the equations for the positions and velocities of the center of mass for different bodies of the
1 1 1 1 1 2
2
𝑇𝑇𝑖𝑖 = 𝑚𝑚𝑖𝑖0 �𝑥𝑥̇ 𝑖𝑖0 2
+ 𝑦𝑦̇ 𝑖𝑖0 2
� + 𝑚𝑚𝑖𝑖1 �𝑥𝑥̇ 𝑖𝑖1 2
+ 𝑦𝑦̇ 𝑖𝑖1 2
� + 𝑚𝑚𝑖𝑖2 �𝑥𝑥̇ 𝑖𝑖2 2
+ 𝑦𝑦̇ 𝑖𝑖2 � + 𝐽𝐽𝑖𝑖1 𝜃𝜃̇𝑖𝑖1
2
+ 𝐽𝐽𝑖𝑖2 �𝜃𝜃̇𝑖𝑖1 + 𝜃𝜃̇𝑖𝑖2 �
2 2 2 2 2
𝐿𝐿𝑖𝑖 = 𝑇𝑇𝑖𝑖
3
𝐿𝐿 = � 𝐿𝐿𝑖𝑖
𝑖𝑖=1
Where
Here, state variables, q = [s1, θ11, θ12, s2, θ21, θ22, s3, θ31, θ32]
In this case,
𝑀𝑀1 0 0
𝑀𝑀 = � 0 𝑀𝑀2 0�
0 0 𝑀𝑀3
𝑉𝑉1
𝑉𝑉 = �𝑉𝑉2 �
𝑉𝑉3
47 | P a g e
1 0 0
⎡0 0 0⎤
⎢0 0 0⎥
⎢ ⎥
⎢0 1 0⎥
𝐸𝐸 = ⎢0 0 0⎥
⎢0 0 0⎥
⎢0 0 1⎥
⎢0 0 0⎥
⎣0 0 0⎦
𝑀𝑀𝑖𝑖
1 1 1
⎡ mi0 + mi1 + mi2 − mi1 li1 si1 − mi2 li1 si1 − mi2 li2 si3 − mi2 li2 si3 ⎤
⎢ 2 2 2 ⎥
1 1 1 1 1 1
= ⎢− mi1 li1 si1 − mi2 li1 si1 − mi2 li2 si3 2 2 2
m l + mi2 li1 + mi2 li2 + Ji1 + Ji2 + mi2 li1 li2 ci2 Ji2 + mi2 l2i2 + mi2 li1 li2 ci2 ⎥
⎢ 2 2 4 i1 i1 4 4 2 ⎥
⎢ 1 1 1 1 ⎥
⎣ − mi2 li2 si3 Ji2 + mi2 l2i2 + mi2 li1 li2 ci2 2
m l + Ji2 ⎦
2 4 2 4 i2 i2
1 1 1
⎡ mi1 li1 θ̇2i1 ci1 + mi2 li1 θ̇2i1 ci1 + mi2 li2 θ̇11 2
ci3 + mi2 li2 θ̇i1 θ̇i2 ci3 + mi2 li2 θ̇2i2 ci3 ⎤
⎢2 2 2 ⎥
1
𝑉𝑉𝑖𝑖 = ⎢ mi2 li1 li2 θ̇i2 si2 (θ̇i2 + 2θ̇i1 ); ⎥
⎢ 2 ⎥
⎢ 1 ⎥
− mi2 li1 li2 θi1 si2 ̇ 2
⎣ 2 ⎦
Equation 5-11 : Dynamic matrices for the 3PRR (9 joint variables set)
where
The E matrix indicates that the three forces applied to the system will be distributed to the 1st, 4th and
7th DOFs of the system, which are the three prismatic joints in the system.
Similar to hand derivation, we can do a symbolic analysis using Maple. The following are the basic
steps:
2. Define the positions of all centers of mass and differentiate them to get their velocities.
3. Find the total kinetic and potential energies in the system and the Lagrangian as their difference.
4. Create Euler Lagrange procedure within Maple according to the Euler Lagrange equation and apply
on obtained Lagrangian with all state variables to compute the dynamic equations of the model.
5. Isolate the double derivative terms from the Euler Lagrange equations to form the mass matrix.
48 | P a g e
6. Subtract out the double derivative terms from the Euler Lagrange equations to form the force vector.
7. Differentiate the position constraints in the system to get the velocity constraints and isolate the
single derivative terms from these differentiated constraints to form the constraint matrix.
8. Use proper substitution and export all matrices and vectors to txt/csv formats for use in Matlab.
9. Import the matrices into Maplesim and compare them with matrices obtained automatically from
Maplesim to get the differences. Verify that the obtained differences are zero.
On comparison of the txt files thus created, we find that the mass matrix, constraint matrix and force
MapleSim is a multi-domain modeling and simulation tool. It does symbolic computation of the
equations of motion and provides them in various forms like the state-space model or the differential
equations form. Additionally, it can be used to analyze a system directly or export the model to a Simulink
model for further analysis. The main features included in the Maplesim model were:
• Three PRR sub-systems (with two links each) were developed and connected to each other
• Inside each PRR system, we have a prismatic joint. Following that, we have two revolute joints
Angles of rotation of base links in global 2𝜋𝜋 4𝜋𝜋 Rotation matrix angles for ref. frames
�0 �
reference frame 3 3 before PRR subsystems
𝐿𝐿 −𝐿𝐿
⎡0 ⎤ Translation vectors for ref. frames before
⎢ 4 4 ⎥
Base locations for 3 prismatic joints
⎢−𝐿𝐿 𝐿𝐿 −𝐿𝐿 ⎥
PRR subsystems
⎣ √3 2√3 2√3⎦
Orientation of each PRR end link with 30◦+(i-1)*120◦, Rotation matrix angles for ref. frames after
After this step, we run a simulation to check the modeling. The simulation results are then compared to
the forward simulation results from Matlab and Simmechanics. This is done in Figure 38.
The next step is to link this model to Maple and extract the underlying equations. Using ‘BuildEQs’
command, we build the equations and extract the mass and constraint matrices and the force vector. Due
to the length of the equations, they have been included in the appendix.
We see that there are only seven variables in the formulation. This is because MapleSim tries to
compute the dynamic equations with the least amount of time and variables. We can change this default
set and force MapleSim to include all the nine variables that we have chosen for prior analysis by giving
initial conditions on them. The new mass and constraint matrices and force vector are exactly matching the
ones derived with hand calculation and are shown in Equation 5-11.
As we can see, the mass matrix and force vector match term by term. This validates both the hand
derivation as well as the MapleSim software results. The constraint matrices still don’t match as they have
been created from Position constraints expressed in different reference frames. Both of them are accurate.
50 | P a g e
5.3. Matlab Kinematic Simulation
Before doing a dynamic simulation, a kinematic simulation was attempted. We use the constraint
Now, we should be done if we can calculate the initial pose of the system.
In order to do that, we start with the required starting point and rotation of the central platform and
calculate its end points. The third link angle of each link is related to the platform angle φ as explained
earlier during kinematic analysis. Then, we calculate the prismatic movement and angle of the first link by
setting up a Newton Raphson method. The condition is that the same point lies on a circle (with its center at
the end point of the platform and radius equal to the link’s length) and on the line joining the two base
points.
Hence, we successfully complete the kinematic analysis of the system. For running the analysis, we
can use either fixed time step solvers (like ode4) or variable time step solvers (like ode45) to get the
following result:
3
2.5
1.5
0.5
-0.5
-1
-1.5
-2.5 -2 -1.5 -1 -0.5 0 0.5 1 1.5 2 2.5
The blue, green and red links represent the 3 different PRR sub-systems. The magenta link represents
the central platform. The bold black curve represents the ellipse that has been traced till this point. The
51 | P a g e
5.4. Simmechanics Forward Dynamics Simulation Comparison
As an additional step to verify that the modeling is correct, we try to simulate the same models with
Simmechanics, a mechanics toolbox for Simulink and Matlab. The following model was created in
-1
-2
-3
-3 -2 -1 0 1 2 3
52 | P a g e
Figure 37 - 3PRR forward dynamics model - Simmechanics
After computing the dynamic matrices, we use the method of Force projection to create the forward
dynamics routine in Matlab. The EOM for a constrained manipulator are expressed as
where, λ is the vector of constraint forces. Combining this equation with the acceleration constraints
we can write,
−1
�𝑀𝑀 𝐴𝐴𝑇𝑇 � �𝑞𝑞̈ � = �𝐸𝐸𝐸𝐸 − 𝑉𝑉 � ⇒ �𝑞𝑞̈ � = �𝑀𝑀 𝐴𝐴𝑇𝑇 � �
𝐸𝐸𝐸𝐸 − 𝑉𝑉
�
𝐴𝐴 0 𝜆𝜆 −𝐴𝐴̇𝑞𝑞̇ 𝜆𝜆 𝐴𝐴 0 −𝐴𝐴̇𝑞𝑞̇
Hence, given the state positions and velocities, we can find the state accelerations.
On comparing the results, we find close matching for the calculated positions and velocities of all
states between the Matlab and Simmechanics forward dynamics simulations. Hence, we can infer that our
53 | P a g e
On comparing the results with MapleSim, we find that the Simmechanics and MapleSim results are a
satisfactory match. The high “error” in the first graph is attributed to the angular positions since for any
angle θ, θ=θ+2π. As a final step before real time implementation, we program this Simmechanics model as
our forward dynamics simulator (or system model) and apply our control techniques on it.
7 -4
x 10
s1d 4
th11d s1d
6 th12d th11d
3
s2d th12d
th21d s2d
th22d 2 th21d
5
s3d th22d
th31d s3d
1
th32d th31d
4 th32d
0
3
-1
2 -2
-3
1
-4
0
-5
-1
0 5 10 15 20 25 30 35 40 0 5 10 15 20 25 30 35 40
(a) (b)
-4 0.8
x 10
6 Tau1
s1 Tau2
th11 0.6 Tau3
th12
4 s2
th21
0.4
th22
s3
2 th31
th32 0.2
0
0
-0.2
-2
-0.4
-4
-0.6
-6 -0.8
0 5 10 15 20 25 30 35 40
0 5 10 15 20 25 30 35 40
(c) (d)
Figure 38 - Maplesim/Matlab Forward dynamics comparison
(a) Difference in state positions calculated (b) Difference in state positions calculated (zoomed in) (c) Difference in state velocities
calculated (d) Torques supplied
In this step, we combine the inverse dynamics from Matlab with the forward dynamics from
The control torque is determined by projecting onto the feasible motion space.
In order to get rid of the last term with unknown λ, we use the fact that S is the null space of A and pre-
In order to converge the initial errors in position/velocities (if any), we use PD control along with this
𝝉𝝉 = (𝑺𝑺𝑻𝑻 𝑬𝑬)−𝟏𝟏 (𝑺𝑺𝑻𝑻 𝑴𝑴𝒒𝒒̈ + 𝑺𝑺𝑻𝑻 𝑽𝑽) + 𝒌𝒌𝒑𝒑 ∙ (𝒛𝒛𝒅𝒅𝒅𝒅𝒅𝒅 − 𝒛𝒛𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂 ) + 𝒌𝒌𝒅𝒅 ∙ (𝒛𝒛̇ 𝒅𝒅𝒅𝒅𝒅𝒅 − 𝒛𝒛̇ 𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂 )
55 | P a g e
The motion profiles required are sine waves of different magnitudes and frequencies and the following
0.005
0.2
0
0
-0.005
-0.2
-0.01
-0.4
-0.015
-0.6
-0.02
-0.025 -0.8
0 5 10 15 20 25 30 35 40 45 50 0 5 10 15 20 25 30 35 40 45 50
(a) (b)
0.8 15
Pos_s 1 Force_s 1
Pos_s 2 Force_s 2
0.6 10
Pos_s 3 Force_s 3
0.4 5
0.2 0
0 -5
-0.2 -10
-0.4 -15
-0.6 -20
-0.8 -25
0 5 10 15 20 25 30 35 40 45 50 0 5 10 15 20 25 30 35 40 45 50
(c) (d)
Figure 40 - Maplesim/Matlab control dynamics simulation
(a) Difference in expected and attained prismatic joint positions (b) difference in expected and attained prismatic joint velocities (c)
Expected prismatic joint positions (d) Torques supplied
56 | P a g e
6. Kinematic and Dynamic Analysis and Control of Hexapod
As discussed in kinematic analysis example in 3.1.2, there are various choices of variable spaces for
Since Maplesim forces those variables into the variable set whose initial conditions are specified,
various possibilities were easily evaluated using the same hexapod model in Maplesim with different sets of
The first set was with all the prismatic joints included in the initial conditions, which led to a 21 variable
workspace.
𝑞𝑞 = [𝑠𝑠1 , 𝑠𝑠2 , 𝑠𝑠3 , 𝑠𝑠4 , 𝑠𝑠5 , 𝑠𝑠6 , 𝑎𝑎𝑎𝑎, 𝑎𝑎𝑎𝑎, 𝑎𝑎𝑎𝑎, 𝑎𝑎1 , 𝑏𝑏1 , 𝑎𝑎2 , 𝑏𝑏2 , 𝑎𝑎3 , 𝑏𝑏3 , 𝑎𝑎4 , 𝑏𝑏4 , 𝑎𝑎5 , 𝑏𝑏5 , 𝑎𝑎6 , 𝑏𝑏6 ]
𝑞𝑞𝑑𝑑𝑑𝑑𝑑𝑑 = [𝑎𝑎𝑎𝑎, 𝑎𝑎𝑎𝑎, 𝑎𝑎𝑎𝑎, 𝑎𝑎1 , 𝑏𝑏1 , 𝑎𝑎2 , 𝑏𝑏2 , 𝑎𝑎3 , 𝑏𝑏3 , 𝑎𝑎4 , 𝑏𝑏4 , 𝑎𝑎5 , 𝑏𝑏5 , 𝑎𝑎6 , 𝑏𝑏6 ], 𝑞𝑞𝑖𝑖𝑖𝑖𝑖𝑖 = [𝑠𝑠1 , 𝑠𝑠2 , 𝑠𝑠3 , 𝑠𝑠4 , 𝑠𝑠5 , 𝑠𝑠6 ]
The second set was with all the prismatic joints and the end effector's position included, which led to a
24 variable analysis. However, because of the way Maplesim calculates the mass matrix, this condition set
led to some zero rows and columns in the evaluated mass matrix, which made it non-invertible. Hence, this
approach was not pursued further in Maplesim. However, Maple programming for the same was easily
done by adapting the 21-variable formulation above and removing the equations relating [xe, ye, ze] to the
𝑞𝑞 = [𝑥𝑥𝑥𝑥, 𝑦𝑦𝑦𝑦, 𝑧𝑧𝑧𝑧, 𝑠𝑠1 , 𝑠𝑠2 , 𝑠𝑠3 , 𝑠𝑠4 , 𝑠𝑠5 , 𝑠𝑠6 , 𝑎𝑎𝑎𝑎, 𝑎𝑎𝑎𝑎, 𝑎𝑎𝑎𝑎, 𝑎𝑎1 , 𝑏𝑏1 , 𝑎𝑎2 , 𝑏𝑏2 , 𝑎𝑎3 , 𝑏𝑏3 , 𝑎𝑎4 , 𝑏𝑏4 , 𝑎𝑎5 , 𝑏𝑏5 , 𝑎𝑎6 , 𝑏𝑏6 ]
𝑞𝑞𝑑𝑑𝑑𝑑𝑑𝑑 = [𝑥𝑥𝑥𝑥, 𝑦𝑦𝑦𝑦, 𝑧𝑧𝑧𝑧, 𝑎𝑎𝑎𝑎, 𝑎𝑎𝑎𝑎, 𝑎𝑎𝑎𝑎, 𝑎𝑎1 , 𝑏𝑏1 , 𝑎𝑎2 , 𝑏𝑏2 , 𝑎𝑎3 , 𝑏𝑏3 , 𝑎𝑎4 , 𝑏𝑏4 , 𝑎𝑎5 , 𝑏𝑏5 , 𝑎𝑎6 , 𝑏𝑏6 ], 𝑞𝑞𝑖𝑖𝑖𝑖𝑖𝑖 = [𝑠𝑠1 , 𝑠𝑠2 , 𝑠𝑠3 , 𝑠𝑠4 , 𝑠𝑠5 , 𝑠𝑠6 ]
𝑞𝑞𝑑𝑑𝑑𝑑𝑑𝑑 = [𝑠𝑠1 , 𝑠𝑠2 , 𝑠𝑠3 , 𝑠𝑠4 , 𝑠𝑠5 , 𝑠𝑠6 , 𝑎𝑎1 , 𝑏𝑏1 , 𝑎𝑎2 , 𝑏𝑏2 , 𝑎𝑎3 , 𝑏𝑏3 , 𝑎𝑎4 , 𝑏𝑏4 , 𝑎𝑎5 , 𝑏𝑏5 , 𝑎𝑎6 , 𝑏𝑏6 ], 𝑞𝑞𝑖𝑖𝑖𝑖𝑖𝑖 = [𝑥𝑥𝑥𝑥, 𝑦𝑦𝑦𝑦, 𝑧𝑧𝑧𝑧, 𝑎𝑎𝑎𝑎, 𝑎𝑎𝑎𝑎, 𝑎𝑎𝑎𝑎]
After deriving the first set of equations in Maple and comparing the results with MapleSim, it is now
possble to eliminate all the universal joint angles from the constraint equation by breaking the mechanism
𝑞𝑞𝑑𝑑𝑑𝑑𝑑𝑑 = [𝑠𝑠1 , 𝑠𝑠2 , 𝑠𝑠3 , 𝑠𝑠4 , 𝑠𝑠5 , 𝑠𝑠6 ], 𝑞𝑞𝑖𝑖𝑖𝑖𝑖𝑖 = [𝑥𝑥𝑥𝑥, 𝑦𝑦𝑦𝑦, 𝑧𝑧𝑧𝑧, 𝑎𝑎𝑎𝑎, 𝑎𝑎𝑎𝑎, 𝑎𝑎𝑎𝑎]
OR
𝑞𝑞𝑑𝑑𝑑𝑑𝑑𝑑 = [𝑥𝑥𝑥𝑥, 𝑦𝑦𝑦𝑦, 𝑧𝑧𝑧𝑧, 𝑎𝑎𝑎𝑎, 𝑎𝑎𝑎𝑎, 𝑎𝑎𝑎𝑎], 𝑞𝑞𝑖𝑖𝑖𝑖𝑖𝑖 = [𝑠𝑠1 , 𝑠𝑠2 , 𝑠𝑠3 , 𝑠𝑠4 , 𝑠𝑠5 , 𝑠𝑠6 ]
The resulting kinematic matrix is found in 11.6.4. In order to validate this manual formulation, we
compute the prismatic joint positions for the same elliptical trajectory using the 24-variable formulation and
0
2
Error in Prismatic Joint Positions
-0.5
1
-1
0
-1.5
2
-1
1 -2
0 -3
-4
-1 2
1.5
1
0.5
0 -5
Y axis -0.5 0 1 2 3 4 5 6 7
-2 -1
-1.5 Time (s)
-2
0.5
Position Constraints
-1
-1.5
-0.5
-2
-1
-1.5 -2.5
-2
0 1 2 3 4 5 6 7 -3
0 1 2 3 4 5 6 7
Time (s)
Time (s)
58 | P a g e
The only matrix derived in this case was the constraint matrix since the angular velocities of the
connecting legs could not be calculated in terms of the end points available. This analysis could be
As described in 3.2, the dynamics of the Hexapod is obtained using the Euler-Lagrange method of
EOM computation. The matrices are fairly complex and are included in full in the appendix.
This model is then run with some initial conditions and a fixed time step solver. This will allow for
59 | P a g e
Figure 43 : Overall model of Hexapod in Simmechanics
The mechanism fails with the slightest of gravity perturbations and is not analyzed further in this work.
A Maplesim simulation was run for 6.28 seconds with a fixed time step of 0.001.
60 | P a g e
The same simulation was run using Matlab code using the same initial conditions and a comparison of
0.5
0.5
0 0
-0.5
-0.5
-1
-1
-1.5
-1.5 -2
0 1 2 3 4 5 6 7 0 1 2 3 4 5 6 7
(a) (b)
-5
Prismatic Joints Positions x 10 Position Constraints
1.5
2.5 1 1
2 2
2 3 3
4 1 4
5 5
1.5 6 6
7
8
1 0.5
9
10
0.5 11
12
0
13
0
14
15
-0.5
-0.5
-1
-1.5 -1
-2
-1.5
0 1 2 3 4 5 6 0 1 2 3 4 5 6 7
(c) (d)
Figure 45 : Maplesim/Matlab forward dynamics comparison (a)Difference in prismatic positions calculated (b) Difference in
prismatic velocities calculated (c) Actual prismatic positions calculated in Matlab (d) Position constraints (convergent)
It is apparent in Figure 45 (a) and (b) that the positions and velocities calculated by Maplesim and
Matlab match very closely (order of 10-6). Further, the position constraints to be maintained in the system
are also seen to converge steadily to zero. This sets the stage for the next step in the process, namely
control using inverse dynamics routine in Matlab and Forward dynamics routine from Maplesim or
Simmechanics.
61 | P a g e
6.4. Control Dynamics Simulation with Simmechanics
Due to problems with the forward dynamics model for the Hexapod in Simmechanics, this step was in
progress at the time this thesis was written and could be included in future work.
To implement real-time control, the articulated model has to be brought reasonably close to the actual
model. Kinematically, the models are quite alike. Hence, one can find the dynamic properties of real
assemblies that are represented by single links in the AMBS and substitute them to get a model that is very
close to real-time. This same code can be then used for real-time implementation.
62 | P a g e
7. MapleSim for Education
MapleSim software was used earlier for automated symbolic kinematic and dynamic modeling and
simulation of the 3PRR and Hexapod models. In light of the ease with which the symbolic expressions for
the EOMs were generated, its productivity during a basic robotics course (covering the basics of concepts
like Lagrangian modeling and PM control) was explored. However, the use of such software in a robotics
A. Currently the use of MapleSim needs “expert users” who can not only model, but also analyze the
results for their correctness. While tutorials are made available by the vendors of these tools, they
are targeted at a more experienced user (typically with a graduate level knowledge of AMBS).
These traditional tutorials may assume a certain level of both mathematical sophistication and
B. Novitiate robotics students may tend to have difficulty understanding both technical (theoretical)
concepts as well as their technological implementation. Moreover, it is crucial that student gleans a
greater insight into the problem and is better equipped to make engineering judgments from the
Our goal is to create a linkage between the Automated Symbolic Computing approach and the traditional analytical
approach so that the student can derive benefits from both – a better understanding of the problem as well as greater proficiency
The implementation is as a series of tutorials with increasing complexity and decreasing support
starting from simple examples like Single Pendulum and Double Pendulum followed by intermediate
examples like the Fourbar and finally leading to the final projects for the course in terms of complex planar
63 | P a g e
8. Future Scope
The real-time implementation of model-based control techniques on the Hexapod could be done by
following the same steps as for the 3PRR. Also, the constant position workspace of parallel platform
manipulators can be generated by modifying the Exact Workspace method with Geometric constraints. The
Exact Workspace method can be considered as a base for optimization algorithms because of its efficiency
and accuracy.
9. Conclusion
During the course of this thesis, a better understanding of the process of modeling, analysis, control
and refinement of articulated multi-body systems was obtained by taking a complicated model, simplifying it
with an articulated approximations and doing analyses on the articulated as well as complete model. We
have succeeded in making an articulated system from a complicated system. Kinematic and dynamic
analyses were done and both kinematic and dynamic control was successfully achieved for this model.
Also, the workspace analysis of parallel manipulators was studied and a new, more efficient method of
10. References
[1] Merlet, J. P., February 2006, Parallel Robots (Solid Mechanics and Its Applications), Springer.
[2] Merlet, J. P., 2006, "Jacobian, Manipulability, Condition Number, and Accuracy of Parallel Robots,"
Journal of Mechanical Design, 128(1), pp. 199-206.
[3] Liu, M. J., Li, C. X., and Li, C. N., 2000, "Dynamics Analysis of the Gough-Stewart Platform
Manipulator," Ieee Transactions on Robotics and Automation, 16(1), pp. 94-98.
[4] Mahmoodi, A., Menhaj, M. B., and Sabzehparvar, M., 2009, "An Efficient Method for Solution of Inverse
Dynamics of Stewart Platform," Aircraft Engineering and Aerospace Technology, 81(5), pp. 398-406.
[5] Vakil, M., Pendar, H., and Zohoor, H., 2008, "Closed-Form Dynamic Equations of the General Stewart
Platform through the Newton-Euler Approach" And "A Newton-Euler Formulation for the Inverse Dynamics
of the Stewart Platform Manipulator"," Mechanism and Machine Theory, 43(10), pp. 1349-1351.
[6] Xi, F., and Sinatra, R., 2002, "Inverse Dynamics of Hexapods Using the Natural Orthogonal
Complement Method," Journal of Manufacturing Systems, 21(2), pp. 73-82.
[7] Sathiyanarayanan, M., 2008, "Analysis of Parallel Platform Architectures for Use in Masticatory Studies,"
M.S. Thesis, SUNY Buffalo, Buffalo.
[8] Meng, J., Liu, G. F., and Li, Z. X., 2007, "A Geometric Theory for Analysis and Synthesis of Sub-6 Dof
Parallel Manipulators," Ieee Transactions on Robotics, 23(4), pp. 625-649.
[9] Staicu, S., 2009, "Inverse Dynamics of the 3-Prr Planar Parallel Robot," Robotics and Autonomous
Systems, 57(5), pp. 556-563.
[10] Staicu, S., 2009, "Power Requirement Comparison in the 3-Rpr Planar Parallel Robot Dynamics,"
Mechanism and Machine Theory, 44(5), pp. 1045-1057.
[11] Saxena, V., Liu, D., Daniel, C. M., and Sutherland, J. W., 1997, "Simulation Study of the Workspace
and Dexterity of a Stewart Platform Based Machine Tool," Proc. Proceedings of the 1997 ASME
64 | P a g e
International Mechanical Engineering Congress and Exposition, November 16, 1997 - November 21, 1997,
Dallas, TX, USA, 61, pp. 617-623.
[12] Bonev, I. A., and Gosselin, C. M., September 10-13, 2000, "Geometric Algorithms for the Computation
of the Constant-Orientation Workspace and Singularity Surfaces of a Special 6-Rus Parallel Manipulator,"
Proc. ASME Design Engineernig Technical Conferences (DETC 2000), Baltimore, MD.
[13] Jiang, Q., and Gosselin, C. M., 2009, "Evaluation and Representation of the Theoretical Orientation
Workspace of the Gough-Stewart Platform," Journal of Mechanisms and Robotics, 1(2), pp. 9.
[14] Merlet, J. P., 1999, "Determination of 6d Workspaces of Gough-Type Parallel Manipulator and
Comparison between Different Geometries," International Journal of Robotics Research, 18(Compendex),
pp. 902-916.
[15] Yoshito Tanaka, I. Y., J. Ishii, M. Wakiyama, 1999, "Evaluation of Workspace in Parallel Machines."
[16] Ryu, I. B. A. J., 1999, "Workspace Analysis of 6-Prrs Parallel Manipulators Based on the Vertex Space
Concept," Proc. ASME Design Technical Conference, , Las Vegas, Nevada.
[17] Pham, N., Joon-Ho, K., and Han-Sung, K., 2008, "Development of a New 6-Dof Parallel-Kinematic
Motion Simulator (Iccas 2008)," Proc. Control, Automation and Systems, 2008. ICCAS 2008. International
Conference on, pp. 2370-2373.
[18] Pernkopf, F., and Husty, M. L., 2004, On Advances in Robot Kinematics, Sestri Levanta, Italy.
[19] H. Shah, S. T., L-F. Lee, and V. Krovi, 2010, "Role of Automated Symbolic Generation of Equations of
Motion in Mechanism and Robotics Education," Proc. ASME 2010 International Design Engineering
Technical Conference, Montreal, Quebec, Canada.
65 | P a g e
11. Appendix
The significant parts in the Hexapod are listed below in the descending order of their characteristic
dimension.
Base platform 1
Hex table 1
Arm 6
Hex table BB 3
Arm BB 3
Gimbal cantilevers 30
l𝑖𝑖1
𝑥𝑥𝑖𝑖0 = 𝑏𝑏𝑥𝑥𝑥𝑥 + ∙ cos
(𝜃𝜃𝑖𝑖1 )
2
l𝑖𝑖1
𝑦𝑦𝑖𝑖0 = 𝑏𝑏𝑦𝑦𝑦𝑦 + ∙ 𝑠𝑠𝑠𝑠𝑠𝑠
(𝜃𝜃𝑖𝑖1 )
2
11-1 | P a g e
𝑙𝑙𝑖𝑖2
𝑥𝑥𝑖𝑖2 = 𝑏𝑏𝑥𝑥𝑥𝑥 + (l𝑖𝑖1 + si ) ∙ cos(𝜃𝜃𝑖𝑖1 ) + ∙ cos(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 )
2
𝑙𝑙𝑖𝑖2
𝑦𝑦𝑖𝑖2 = 𝑏𝑏𝑦𝑦𝑦𝑦 + (l𝑖𝑖1 + si ) ∙ sin(𝜃𝜃𝑖𝑖1 ) + ∙ sin(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 )
2
Center of platform:
l𝑖𝑖1
𝑥𝑥𝑖𝑖0̇ = − ∙ 𝑠𝑠𝑠𝑠𝑠𝑠
(𝜃𝜃𝑖𝑖1 )
2
l𝑖𝑖1
𝑦𝑦𝑖𝑖0̇ = ∙ cos
(𝜃𝜃𝑖𝑖1 )
2
𝑙𝑙𝑖𝑖2
𝑥𝑥𝑖𝑖2̇ = −(l𝑖𝑖1 + si ) ∙ 𝑠𝑠𝑠𝑠𝑠𝑠(𝜃𝜃𝑖𝑖1 ) + ṡ i ∙ cos
(𝜃𝜃𝑖𝑖1 ) − ∙ sin(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 ) ∙ (𝜃𝜃𝑖𝑖1̇ + 𝜃𝜃𝑖𝑖2̇ )
2
𝑙𝑙𝑖𝑖2
𝑦𝑦𝑖𝑖2̇ = (l𝑖𝑖1 + si ) ∙ cos(𝜃𝜃𝑖𝑖1̇ ) + ṡ i ∙ sin
(𝜃𝜃𝑖𝑖1 ) + ∙ cos(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 ) ∙ (𝜃𝜃𝑖𝑖1̇ + 𝜃𝜃𝑖𝑖2̇ )
2
11-2 | P a g e
11.3. Gauss Divergence Theorem Calculations
A circular arc is represented by its center (xc,yc), its radius rc and the bounding angles θ1 and θ2.
𝑛𝑛� = cos(𝜃𝜃) 𝑖𝑖̂ + sin(𝜃𝜃) 𝑗𝑗̂, 𝑥𝑥 = 𝑥𝑥𝑐𝑐 + 𝑟𝑟𝑐𝑐 ∙ cos(𝜃𝜃) , 𝑦𝑦 = 𝑦𝑦𝑐𝑐 + 𝑟𝑟𝑐𝑐 ∙ sin(𝜃𝜃) , 𝑑𝑑𝑑𝑑 = 𝑟𝑟𝑐𝑐 ∙ 𝑑𝑑𝑑𝑑
(𝑥𝑥𝑐𝑐 + 𝑟𝑟𝑐𝑐 ∙ cos(𝜃𝜃)) ∙ cos(𝜃𝜃) + (𝑦𝑦𝑐𝑐 + 𝑟𝑟𝑐𝑐 ∙ sin(𝜃𝜃)) ∙ sin(𝜃𝜃) 𝑥𝑥𝑐𝑐 ∙ cos(𝜃𝜃) + 𝑦𝑦𝑐𝑐 ∙ sin(𝜃𝜃) + 𝑟𝑟𝑐𝑐
𝐹𝐹 ∙ 𝑛𝑛� = =
2 2
1 𝜃𝜃2
�𝑑𝑑𝑑𝑑 = � (𝑥𝑥𝑐𝑐 ∙ cos(𝜃𝜃) + 𝑦𝑦𝑐𝑐 ∙ sin(𝜃𝜃) + 𝑟𝑟𝑐𝑐 ) ∙ 𝑟𝑟𝑐𝑐 ∙ 𝑑𝑑𝑑𝑑
� 𝑭𝑭. 𝒏𝒏
𝑑𝑑𝑑𝑑 2 𝜃𝜃1
1
= �𝑟𝑟 ∙ 𝑥𝑥 ∙ (sin(𝜃𝜃2 ) − sin(𝜃𝜃1 )) − 𝑟𝑟𝑐𝑐 ∙ 𝑦𝑦𝑐𝑐 ∙ (cos(𝜃𝜃2 ) − cos(𝜃𝜃1 )) + 𝑟𝑟2𝑐𝑐 ∙ (𝜃𝜃2 − 𝜃𝜃1 )�
2 𝑐𝑐 𝑐𝑐
Equation 11-1 : Area under circular arc using Gauss divergence theorem
11.3.2. Line
𝑥𝑥1 − 𝑥𝑥2 y2 − y1
𝑑𝑑 = �(𝑥𝑥2 − 𝑥𝑥1)2 + (𝑦𝑦2 − 𝑦𝑦1)2 , 𝑛𝑛� = � � 𝑖𝑖̂ + ( )𝑗𝑗̂
𝑑𝑑 d
�𝑥𝑥1 + 𝑏𝑏1 ∙ (𝑥𝑥1 − 𝑥𝑥2)� ∙ (𝑥𝑥1 − 𝑥𝑥2) + �𝑦𝑦1 + 𝑏𝑏1 ∙ (𝑦𝑦2 − 𝑦𝑦1)� ∙ (𝑦𝑦2 − 𝑦𝑦1) 𝑥𝑥12 − 𝑥𝑥1 ∙ 𝑥𝑥2 + 𝑦𝑦1 ∙ 𝑦𝑦2 − 𝑦𝑦12 + 𝑏𝑏1 ∙ 𝑑𝑑 2
𝐹𝐹 ∙ 𝑛𝑛� = =
2 ∙ 𝑑𝑑 2 ∙ 𝑑𝑑
𝑏𝑏1𝑏𝑏
1
�𝑑𝑑𝑑𝑑 =
� 𝑭𝑭. 𝒏𝒏 � �𝑥𝑥21 − 𝑥𝑥1 ∙ 𝑥𝑥2 + 𝑦𝑦1 ∙ 𝑦𝑦2 − 𝑦𝑦21 + 𝑏𝑏1 ∙ 𝑑𝑑2 � ∙ 𝑑𝑑𝑑𝑑1
𝑑𝑑𝑑𝑑 2 ∙ 𝑑𝑑 𝑏𝑏1𝑎𝑎
�𝑥𝑥21 − 𝑥𝑥1 ∙ 𝑥𝑥2 + 𝑦𝑦1 ∙ 𝑦𝑦2 − 𝑦𝑦21 � ∙ (𝑏𝑏1𝑏𝑏 − 𝑏𝑏1𝑎𝑎) 𝑑𝑑 ∙ (𝑏𝑏1𝑏𝑏2 − 𝑏𝑏1𝑎𝑎2 )
= +
2 ∙ 𝑑𝑑 4
Equation 11-2 : Area under line using Gauss divergence theorem
11-3 | P a g e
11.4. Maple Code for Hexapod Dynamic Analysis
This is a discussion of the code used to generate the dynamic EOMs for the Hexapod in Maple.
The next step is to declare the transformation matrices from the ground frame to the reference frames
Ai (as shown in Figure 16). Also, T7 refers to the transformation matrix relating only the rotations of the end-
The next step is the calculation of the positions of the centers of mass for the six legs. It is stored in
X2i. X2ai refer to the position of the end of the six legs connected to the platform.
The next step is the calculation of the position of the center and the end-points of the platform. The
end-point is also calculated by using Equation 4-4(1) on the second leg. This is done to match the
11-4 | P a g e
The next step is to calculate the linear and angular velocities of the six legs and the platform. The
linear velocities are computed easily by differentiation and the angular velocities are computed by using the
twist formulation.
The next step is the computation of the Lagrangian of the system, which is the difference of the kinetic
and potential energies of the system. To do this, the mass, position, Moment of inertia about principal axes
11-5 | P a g e
and angular velocity of all the masses are stored in a matrix, which is then run through a loop in the
The next step is the declaration of the variable space, its derivatives and double-derivatives.
The next step is to run through all the variables and store the resulting Euler-Lagrange equations in a
matrix. This is done using either the “EulerLagrange” command in Maple or using a user-written function as
shown here.
11-6 | P a g e
The next step is the extraction of the mass matrix and force vector from the Euler-Lagrange equations.
The mass matrix is extracted by isolating the terms relating to the double-derivatives of the states from
each equation. The force vector is computed as the difference of the total equation and the product of the
The last step is the creation of the position constraints in the system. This is done by equating the
coordinates of the end-points of the platform and converting them to the reference frames at those points.
11.5.1. Substitutions
cos(th11)=c11; cos(th12)=c12; cos(th21)=c21; cos(th22)=c22; cos(th31)=c31; cos(th32)=c32;
sin(th11)=s11; sin(th12)=s12; sin(th21)=s21; sin(th22)=s22; sin(th31)=s31; sin(th32)=s32;
cos(4/3*pi)=c43pi; sin(4/3*pi)=s43pi;
11-7 | P a g e
11.5.2. 7 variable set
11-8 | P a g e
l32*cal3*c31*c12*c43pi+l32*cal3*c31*s12*s43pi+l32*cal3*s31*s12*c43pi+l32*cal3*s31*c12*s43pi+l32*sal3*s31*c12*c43pi-
l32*sal3*s31*s12*s43pi+l32*sal3*c31*s12*c43pi+l32*sal3*c31*c12*s43pi]’;
11.6.1. Substitutions
sal1=sin(al(1)); sal2=sin(al(2)); sal3=sin(al(3));
cal1=cos(al(1)); cal2=cos(al(2)); cal3=cos(al(3));
sax=sin(ax); say=sin(ay); saz=sin(az);
cax=cos(ax); cay=cos(ay); caz=cos(az);
ca1=cos(a1); ca2=cos(a2); ca3=cos(a3); ca4=cos(a4); ca5=cos(a5); ca6=cos(a6);
sa1=sin(a1); sa2=sin(a2); sa3=sin(a3); sa4=sin(a4); sa5=sin(a5); sa6=sin(a6);
cb1=cos(b1); cb2=cos(b2); cb3=cos(b3); cb4=cos(b4); cb5=cos(b5); cb6=cos(b6);
sb1=sin(b1); sb2=sin(b2); sb3=sin(b3); sb4=sin(b4); sb5=sin(b5); sb6=sin(b6);
11-9 | P a g e
1/3*M*l2*bd2^2*l*sq3*sal1*sb2*cax*saz-2/3*M*l2*ad2*l*sq3*sb2*sax*say*caz*cal1*sa2*bd2-
2/3*M*l2*ad2*l*sq3*sb2*cax*saz*cal1*sa2*bd2-1/3*M*l2*bd2^2*l*sq3*cb2*cay*caz*sal1*ca2-
1/3*M*l2*ad2^2*l*sq3*cb2*cay*caz*sal1*ca2+1/3*M*l2*bd2^2*l*sq3*cb2*cax*saz*cal1*ca2+1/3*M*l2*ad2^2*l*sq3*cb2*cax*saz*cal1*ca
2+1/3*M*l2*bd2^2*l*sq3*cb2*caz*sax*ca2*say*cal1+1/3*M*l2*ad2^2*l*sq3*cb2*caz*sax*ca2*say*cal1-
2/3*M*l2*bd2*l*sq3*ca2*sb2*cax*say*caz*ad2+2/3*M*l2*bd2*l*sq3*ca2*sb2*sax*saz*ad2;
-1/2*m1*l1^2*ad1*bd1*sb1*cb1-2*ad1*Jx*bd1*sb1*cb1+2*ad1*Jy*bd1*sb1*cb1+1/2*m1*g*l1*ca1*cb1;
ad1^2*Jx*sb1*cb1-ad1^2*Jy*sb1*cb1+1/4*m1*l1^2*ad1^2*sb1*cb1-1/2*m1*g*l1*sa1*sb1;
-2*ad2*Jx*bd2*sb2*cb2+2*ad2*Jy*bd2*sb2*cb2-1/2*m2*l2^2*ad2*bd2*sb2*cb2-
2*M*l2^2*ad2*bd2*sb2*cb2+1/2*m2*g*l2*ca2*cb2+M*g*l2*ca2*cb2+2/3*M*l2*ayd*l*sq3*cb2*say*caz*sal1*sa2*azd+1/3*M*l2*ayd^2*l*sq3
*cb2*cay*saz*sal1*sa2+1/3*M*l2*azd^2*l*sq3*cb2*cay*saz*sal1*sa2+2/3*M*l2*l*sq3*axd*cb2*caz*cax*sa2*say*cal1*azd+1/3*M*l2*azd
^2*l*sq3*cb2*cax*caz*cal1*sa2+1/3*M*l2*l*sq3*axd^2*cb2*cax*caz*cal1*sa2+2/3*M*l2*ayd*l*sq3*cb2*caz*sax*cay*cal1*sa2*azd+2/3*
M*l2*l*sq3*axd*cb2*saz*cax*sa2*cay*cal1*ayd-2/3*M*l2*l*sq3*axd*cb2*sax*saz*cal1*sa2*azd-
1/3*M*l2*ayd^2*l*sq3*cb2*sax*say*saz*cal1*sa2-1/3*M*l2*azd^2*l*sq3*cb2*sax*say*saz*cal1*sa2-
1/3*M*l2*l*sq3*axd^2*cb2*sax*say*saz*cal1*sa2-1/3*M*l2*azd^2*l*sq3*ca2*cb2*cax*say*saz-
1/3*M*l2*l*sq3*axd^2*ca2*cb2*cax*say*saz-1/3*M*l2*ayd^2*l*sq3*ca2*cb2*cax*say*saz-2/3*M*l2*l*sq3*axd*ca2*cb2*cax*saz*azd-
2/3*M*l2*ayd*l*sq3*ca2*cb2*saz*sax*cay*axd-1/3*M*l2*azd^2*l*sq3*ca2*cb2*sax*caz-1/3*M*l2*l*sq3*axd^2*ca2*cb2*sax*caz-
2/3*M*l2*azd*l*sq3*ca2*cb2*sax*say*caz*axd+2/3*M*l2*ayd*l*sq3*ca2*cb2*caz*cax*cay*azd;
2/3*M*l2*azd*l*sq3*sal1*cb2*cax*say*caz*axd+1/3*M*l2*azd^2*l*sq3*cax*caz*sal1*cb2+1/3*M*l2*l*sq3*axd^2*cax*caz*sal1*cb2+2
/3*M*l2*azd*l*sq3*sal1*cb2*caz*sax*cay*ayd-1/3*M*l2*ayd^2*l*sq3*cay*saz*cal1*cb2-1/3*M*l2*azd^2*l*sq3*cay*saz*cal1*cb2-
2/3*M*l2*azd*l*sq3*cal1*cb2*say*caz*ayd+1/3*M*l2*l*sq3*axd^2*cax*say*saz*sa2*sb2+1/3*M*l2*ayd^2*l*sq3*cax*say*saz*sa2*sb2+2/
3*M*l2*l*sq3*axd*sa2*sb2*cax*saz*azd+1/3*M*l2*azd^2*l*sq3*cax*say*saz*sa2*sb2+2/3*M*l2*azd*l*sq3*sa2*sb2*sax*say*caz*axd+1/3
*M*l2*l*sq3*axd^2*sax*caz*sa2*sb2+1/3*M*l2*azd^2*l*sq3*sax*caz*sa2*sb2-
2/3*M*l2*ayd*l*sq3*sa2*sb2*caz*cax*cay*azd+2/3*M*l2*ayd*l*sq3*sa2*sb2*saz*sax*cay*axd+2/3*M*l2*l*sq3*axd*sal1*cb2*saz*cax*ca
y*ayd-2/3*M*l2*l*sq3*axd*sal1*cb2*sax*saz*azd-1/3*M*l2*ayd^2*l*sq3*sax*say*saz*sal1*cb2-
1/3*M*l2*azd^2*l*sq3*sax*say*saz*sal1*cb2-
1/3*M*l2*l*sq3*axd^2*sax*say*saz*sal1*cb2+2/3*M*l2*ayd*l*sq3*sb2*say*caz*sal1*ca2*azd+2/3*M*l2*l*sq3*axd*sb2*saz*cax*ca2*cay
*cal1*ayd-2/3*M*l2*l*sq3*axd*sb2*sax*saz*cal1*ca2*azd-1/3*M*l2*ayd^2*l*sq3*sax*say*saz*cal1*ca2*sb2-
1/3*M*l2*azd^2*l*sq3*sax*say*saz*cal1*ca2*sb2-
1/3*M*l2*l*sq3*axd^2*sax*say*saz*cal1*ca2*sb2+2/3*M*l2*l*sq3*axd*sb2*caz*cax*ca2*say*cal1*azd+1/4*m2*l2^2*ad2^2*sb2*cb2-
1/2*m2*g*l2*sa2*sb2+M*l2^2*ad2^2*sb2*cb2-M*g*l2*sa2*sb2+ad2^2*Jx*sb2*cb2-
ad2^2*Jy*sb2*cb2+1/3*M*l2*l*sq3*axd^2*cax*caz*cal1*ca2*sb2+1/3*M*l2*azd^2*l*sq3*cax*caz*cal1*ca2*sb2+2/3*M*l2*ayd*l*sq3*sb2*
caz*sax*ca2*cay*cal1*azd+1/3*M*l2*ayd^2*l*sq3*cay*saz*sal1*ca2*sb2+1/3*M*l2*azd^2*l*sq3*cay*saz*sal1*ca2*sb2;
-1/2*m3*l3^2*ad3*bd3*sb3*cb3-2*ad3*Jx*bd3*sb3*cb3+2*ad3*Jy*bd3*sb3*cb3+1/2*m3*g*l3*ca3*cb3;
1/4*m3*l3^2*ad3^2*sb3*cb3+ad3^2*Jx*sb3*cb3-ad3^2*Jy*sb3*cb3-1/2*m3*g*l3*sa3*sb3;
-1/2*m4*l4^2*ad4*bd4*sb4*cb4-2*ad4*Jx*bd4*sb4*cb4+2*ad4*Jy*bd4*sb4*cb4+1/2*m4*g*l4*ca4*cb4;
ad4^2*Jx*sb4*cb4-ad4^2*Jy*sb4*cb4+1/4*m4*l4^2*ad4^2*sb4*cb4-1/2*m4*g*l4*sa4*sb4;
-1/2*m5*l5^2*ad5*bd5*sb5*cb5-2*ad5*Jx*bd5*sb5*cb5+2*ad5*Jy*bd5*sb5*cb5+1/2*m5*g*l5*ca5*cb5;
ad5^2*Jx*sb5*cb5-ad5^2*Jy*sb5*cb5+1/4*m5*l5^2*ad5^2*sb5*cb5-1/2*m5*g*l5*sa5*sb5;
-1/2*m6*l6^2*ad6*bd6*sb6*cb6-2*ad6*Jx*bd6*sb6*cb6+2*ad6*Jy*bd6*sb6*cb6+1/2*m6*g*l6*ca6*cb6;
-1/2*m6*g*l6*sa6*sb6+ad6^2*Jx*sb6*cb6-ad6^2*Jy*sb6*cb6+1/4*m6*l6^2*ad6^2*sb6*cb6];
11-10 | P a g e
1/3*M*l2*l*sq3*ca2*cb2*saz*cax*cay+1/3*M*l2*l*sq3*cb2*say*saz*sal1*sa2+1/3*M*l2*l*sq3*cb2*saz*sax*cay*cal1*sa2, -
1/3*M*l2*l*sq3*cal1*cb2*say*saz+1/3*M*l2*l*sq3*sb2*saz*sax*ca2*cay*cal1+1/3*M*l2*l*sq3*sb2*say*saz*sal1*ca2+1/3*M*l2*l*sq3*s
al1*cb2*saz*sax*cay-1/3*M*l2*l*sq3*sa2*sb2*saz*cax*cay, 0, 0, 0, 0, 0, 0, 0, 0;
0, -1/3*M*l*sq3*cay*caz*cal1-1/3*M*l*sq3*cax*saz*sal1-1/3*M*l*sq3*sax*say*caz*sal1, 0, 0, 0, 0, 1/3*M*l^2*say+JZ*say, 0,
JZ+1/3*M*l^2, 0, 0, 1/3*M*l2*l*sq3*cb2*sax*say*caz*cal1*sa2+1/3*M*l2*l*sq3*ca2*cb2*cax*say*caz-
1/3*M*l2*l*sq3*cb2*cay*caz*sal1*sa2+1/3*M*l2*l*sq3*cb2*cax*saz*cal1*sa2-1/3*M*l2*l*sq3*ca2*cb2*sax*saz,
1/3*M*l2*l*sq3*cax*saz*sal1*cb2-
1/3*M*l2*l*sq3*cay*caz*sal1*ca2*sb2+1/3*M*l2*l*sq3*cay*caz*cal1*cb2+1/3*M*l2*l*sq3*cax*saz*cal1*ca2*sb2+1/3*M*l2*l*sq3*sax*s
az*sa2*sb2+1/3*M*l2*l*sq3*sax*say*caz*sal1*cb2+1/3*M*l2*l*sq3*sax*say*caz*cal1*ca2*sb2-1/3*M*l2*l*sq3*cax*say*caz*sa2*sb2,
0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 1/4*m1*l1^2*cb1^2+Jx*cb1^2-Jy*cb1^2+Jy, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
-1/2*m1*l1*cb1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1/4*m1*l1^2+Jz, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 1/3*M*l2*l*sq3*cb2*sax*caz*cal1*sa2-
1/3*M*l2*l*sq3*ca2*cb2*sax*say*saz+1/3*M*l2*l*sq3*ca2*cb2*cax*caz+1/3*M*l2*l*sq3*cb2*saz*cax*sa2*say*cal1,
1/3*M*l2*l*sq3*ca2*cb2*saz*cax*cay+1/3*M*l2*l*sq3*cb2*say*saz*sal1*sa2+1/3*M*l2*l*sq3*cb2*saz*sax*cay*cal1*sa2,
1/3*M*l2*l*sq3*cb2*sax*say*caz*cal1*sa2+1/3*M*l2*l*sq3*ca2*cb2*cax*say*caz-
1/3*M*l2*l*sq3*cb2*cay*caz*sal1*sa2+1/3*M*l2*l*sq3*cb2*cax*saz*cal1*sa2-1/3*M*l2*l*sq3*ca2*cb2*sax*saz, 0, 0, Jx*cb2^2-
Jy*cb2^2+Jy+1/4*m2*l2^2*cb2^2+M*l2^2*cb2^2, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, -1/2*m2*l2*cb2-M*l2*cb2, 0, 0, 0, 0, -
1/3*M*l2*l*sq3*sa2*sb2*cax*caz+1/3*M*l2*l*sq3*sb2*sax*caz*cal1*ca2+1/3*M*l2*l*sq3*sb2*saz*cax*ca2*say*cal1+1/3*M*l2*l*sq3*sa
l1*cb2*sax*caz+1/3*M*l2*l*sq3*sal1*cb2*cax*say*saz+1/3*M*l2*l*sq3*sa2*sb2*sax*say*saz, -
1/3*M*l2*l*sq3*cal1*cb2*say*saz+1/3*M*l2*l*sq3*sb2*saz*sax*ca2*cay*cal1+1/3*M*l2*l*sq3*sb2*say*saz*sal1*ca2+1/3*M*l2*l*sq3*s
al1*cb2*saz*sax*cay-1/3*M*l2*l*sq3*sa2*sb2*saz*cax*cay, 1/3*M*l2*l*sq3*cax*saz*sal1*cb2-
1/3*M*l2*l*sq3*cay*caz*sal1*ca2*sb2+1/3*M*l2*l*sq3*cay*caz*cal1*cb2+1/3*M*l2*l*sq3*cax*saz*cal1*ca2*sb2+1/3*M*l2*l*sq3*sax*s
az*sa2*sb2+1/3*M*l2*l*sq3*sax*say*caz*sal1*cb2+1/3*M*l2*l*sq3*sax*say*caz*cal1*ca2*sb2-1/3*M*l2*l*sq3*cax*say*caz*sa2*sb2,
0, 0, 0, 1/4*m2*l2^2+M*l2^2+Jz, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1/4*m3*l3^2*cb3^2+Jx*cb3^2-Jy*cb3^2+Jy, 0, 0, 0, 0, 0, 0, 0;
0, 0, -1/2*m3*l3*cb3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1/4*m3*l3^2+Jz, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1/4*m4*l4^2*cb4^2+Jy+Jx*cb4^2-Jy*cb4^2, 0, 0, 0, 0, 0;
0, 0, 0, -1/2*m4*l4*cb4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, Jz+1/4*m4*l4^2, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, Jy+1/4*m5*l5^2*cb5^2+Jx*cb5^2-Jy*cb5^2, 0, 0, 0;
0, 0, 0, 0, -1/2*m5*l5*cb5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, Jz+1/4*m5*l5^2, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, Jy+1/4*m6*l6^2*cb6^2+Jx*cb6^2-Jy*cb6^2, 0;
0, 0, 0, 0, 0, -1/2*m6*l6*cb6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1/4*m6*l6^2+Jz];
11-11 | P a g e
0, 0, 0, 0, 0, 0, 0, 0, 0, -l3*sal2*sa3*cb3, l3*cal2*sa3*cb3, -ca3*cb3*l3, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, -l3*(-cal2*cb3+sal2*ca3*sb3), l3*(sal2*cb3+cal2*ca3*sb3), sa3*sb3*l3, 0, 0, 0;
l4*sal2*sa4*cb4, -l4*cal2*sa4*cb4, ca4*cb4*l4, 0, 0, 0, 0, 0, 0, l4*sal2*sa4*cb4, -l4*cal2*sa4*cb4, ca4*cb4*l4, 0, 0, 0;
l4*(-cal2*cb4+sal2*ca4*sb4), -l4*(sal2*cb4+cal2*ca4*sb4), -sa4*sb4*l4, 0, 0, 0, 0, 0, 0, l4*(-cal2*cb4+sal2*ca4*sb4), -
l4*(sal2*cb4+cal2*ca4*sb4), -sa4*sb4*l4, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -l5*sal3*sa5*cb5, l5*cal3*sa5*cb5, -ca5*cb5*l5;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -l5*(-cal3*cb5+sal3*ca5*sb5), l5*(sal3*cb5+cal3*ca5*sb5), sa5*sb5*l5;
0, 0, 0, l6*sal3*sa6*cb6, -l6*cal3*sa6*cb6, ca6*cb6*l6, 0, 0, 0, 0, 0, 0, l6*sal3*sa6*cb6, -l6*cal3*sa6*cb6, ca6*cb6*l6;
0, 0, 0, -l6*(cal3*cb6-sal3*ca6*sb6), -l6*(sal3*cb6+cal3*ca6*sb6), -sa6*sb6*l6, 0, 0, 0, 0, 0, 0, -l6*(cal3*cb6-
sal3*ca6*sb6), -l6*(sal3*cb6+cal3*ca6*sb6), -sa6*sb6*l6]';
11-12 | P a g e
0, 0, 0, -1/2*m1*l1*sb1, 0, 0, 0, 0, 0, 0, 0, 0, 0, Jx+1/4*m1*l1^2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, Jz-1/4*m2*l2^2*cb2^2+1/4*m2*l2^2+Jy*cb2^2-Jz*cb2^2, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, -1/2*m2*l2*sb2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1/4*m2*l2^2+Jx, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/4*m3*l3^2*cb3^2+1/4*m3*l3^2+Jy*cb3^2-Jz*cb3^2+Jz, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, -1/2*m3*l3*sb3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1/4*m3*l3^2+Jx, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -Jz*cb4^2+Jz+Jy*cb4^2+1/4*m4*l4^2-1/4*m4*l4^2*cb4^2, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, -1/2*m4*l4*sb4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1/4*m4*l4^2+Jx, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1/4*m5*l5^2+Jy*cb5^2-Jz*cb5^2+Jz-1/4*m5*l5^2*cb5^2, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, -1/2*m5*l5*sb5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, Jx+1/4*m5*l5^2, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/4*m6*l6^2*cb6^2+1/4*m6*l6^2+Jy*cb6^2-Jz*cb6^2+Jz, 0;
0, 0, 0, 0, 0, 0, 0, 0, -1/2*m6*l6*sb6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1/4*m6*l6^2+Jx];
11-13 | P a g e
11.6.4. 6 joint variables + 6 task variables set
11-14 | P a g e
cal3*s6*say*saz*3^(1/2)+3*say*caz*xe-xa3*say*saz*3^(1/2)+3*ya3*sax*cay*caz-3*cal3*s6*say*caz+sal3*s6*3^(1/2)*sax*cay*saz-
3*xa3*say*caz+3*sal3*s6*sax*cay*caz);
-2/3*l*3^(1/2)*(-cay*caz*xe-cax*saz*ye-sax*saz*ze-
sax*say*caz*ye+cax*say*caz*ze+sax*say*caz*ya1+cal1*s1*cay*caz+sal1*s1*cax*saz+cay*caz*xa1+cax*saz*ya1+sal1*s1*sax*say*caz),-
2/3*l*3^(1/2)*(-cay*caz*xe-cax*saz*ye-sax*saz*ze-
sax*say*caz*ye+cax*say*caz*ze+sax*say*caz*ya1+cay*caz*cal1*s2+cax*saz*sal1*s2+sax*say*caz*sal1*s2+cay*caz*xa1+cax*saz*ya1),1
/3*l*(-3*sax*say*saz*ye+3*cax*say*saz*ze-cay*caz*3^(1/2)*xe-3^(1/2)*cax*saz*ye-3^(1/2)*sax*saz*ze-
3^(1/2)*sax*say*caz*ye+3^(1/2)*cax*say*caz*ze+cal2*s3*cay*caz*3^(1/2)+3*sal2*s3*sax*say*saz+sal2*s3*3^(1/2)*cax*saz+sal2*s3*
3^(1/2)*sax*say*caz-3*cay*saz*xe+3*cax*caz*ye+3*sax*caz*ze+3*cal2*s3*cay*saz-
3*sal2*s3*cax*caz+xa2*cay*caz*3^(1/2)+3*sax*say*saz*ya2+ya2*3^(1/2)*cax*saz+3*cay*saz*xa2-
3*cax*caz*ya2+ya2*3^(1/2)*sax*say*caz),1/3*l*(-3*sax*say*saz*ye+3*cax*say*saz*ze-cay*caz*3^(1/2)*xe-3^(1/2)*cax*saz*ye-
3^(1/2)*sax*saz*ze-3^(1/2)*sax*say*caz*ye+3^(1/2)*cax*say*caz*ze-
3*cay*saz*xe+3*cax*caz*ye+3*sax*caz*ze+xa2*cay*caz*3^(1/2)+3*sax*say*saz*ya2+ya2*3^(1/2)*cax*saz+3*cay*saz*cal2*s4-
3*cax*caz*sal2*s4+sal2*s4*3^(1/2)*sax*say*caz+3*cay*saz*xa2-
3*cax*caz*ya2+cal2*s4*cay*caz*3^(1/2)+3*sax*say*saz*sal2*s4+sal2*s4*3^(1/2)*cax*saz+ya2*3^(1/2)*sax*say*caz),1/3*l*(3*sax*sa
y*saz*ye-3*cax*say*saz*ze-cay*caz*3^(1/2)*xe-3^(1/2)*cax*saz*ye-3^(1/2)*sax*saz*ze-
3^(1/2)*sax*say*caz*ye+3^(1/2)*cax*say*caz*ze+ya3*3^(1/2)*sax*say*caz+3*cay*saz*xe-3*cax*caz*ye-
3*sax*caz*ze+xa3*cay*caz*3^(1/2)-3*sax*say*saz*ya3+ya3*3^(1/2)*cax*saz-
3*cal3*s5*cay*saz+3*sal3*s5*cax*caz+sal3*s5*3^(1/2)*sax*say*caz-3*cay*saz*xa3+3*cax*caz*ya3+cal3*s5*cay*caz*3^(1/2)-
3*sal3*s5*sax*say*saz+sal3*s5*3^(1/2)*cax*saz),1/3*l*(3*sax*say*saz*ye-3*cax*say*saz*ze-cay*caz*3^(1/2)*xe-
3^(1/2)*cax*saz*ye-3^(1/2)*sax*saz*ze-
3^(1/2)*sax*say*caz*ye+3^(1/2)*cax*say*caz*ze+ya3*3^(1/2)*sax*say*caz+cal3*s6*cay*caz*3^(1/2)+3*cay*saz*xe-3*cax*caz*ye-
3*sax*caz*ze+xa3*cay*caz*3^(1/2)-3*sax*say*saz*ya3+ya3*3^(1/2)*cax*saz-
3*cay*saz*cal3*s6+3*cax*caz*sal3*s6+sal3*s6*3^(1/2)*sax*say*caz-3*cay*saz*xa3+3*cax*caz*ya3-
3*sax*say*saz*sal3*s6+sal3*s6*3^(1/2)*cax*saz)];
11-15 | P a g e