KINEMATIC, DYNAMIC and WORKSPACE ANALYSIS of a novel 6 …€¦ · I also thank the ARMLAB members...
Transcript of KINEMATIC, DYNAMIC and WORKSPACE ANALYSIS of a novel 6 …€¦ · I also thank the ARMLAB members...
KINEMATIC, DYNAMIC AND WORKSPACE ANALYSIS OF A NOVEL 6-DOF PARALLEL MANIPULATOR
by
Hrishi L. Shah
Aug 11, 2010
A thesis submitted to the
Faculty of the Graduate School of
the University at Buffalo, State University of New York
in partial fulfillment of the requirements for the
degree of
Master of Science
Department of Mechanical and Aerospace Engineering
ii | P a g e
8/11/2010
MAE 560 - KRO Hrishi Shah
SUNY BUFFALO
KINEMATIC, DYNAMIC AND WORKSPACE ANALYSIS OF A NOVEL 6-DOF PARALLEL MANIPULATOR
iii | P a g e
To my parents and friends.
You complete me.
iv | P a g e
Acknowledgement
I express my sincerest gratitude for my advisor, Dr. Krovi, for the constant guidance and support
throughout the course of my MS degree, especially during the thesis.
I also express my thanks to Dr. Roger Mayne and Dr. John Crassidis for being my committee members
and critiquing my thesis report.
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
distant from home.
Thank you all.
v | P a g e
Table of Contents
Acknowledgement ................................................................................................................................... iv
Table of Contents ..................................................................................................................................... v
Table of Figures ....................................................................................................................................... ix
Table of Equations................................................................................................................................... xi
Abstract ..................................................................................................................................................... 1
1. Introduction ....................................................................................................................................... 3
1.1. Case Study: Hexapod Parallel Platform Manipulator .................................................................... 4
1.2. Goals of this work ......................................................................................................................... 5
1.3. Model Simplification ..................................................................................................................... 6
1.4. Organization of the rest of the Thesis ........................................................................................... 8
2. Literature Review .............................................................................................................................. 9
2.1. Related work ................................................................................................................................ 9
2.1.1. Modeling and Analysis of parallel manipulators ..................................................................... 9
2.1.2. Workspace Analysis of parallel manipulators ....................................................................... 10
3. Mathematical and Technological Tools ......................................................................................... 12
3.1. Kinematic Analysis ..................................................................................................................... 12
3.1.1. Framework .......................................................................................................................... 12
3.1.2. Example: 3-RPR.................................................................................................................. 13
3.1.2.1. Nomenclature ................................................................................................................. 13
3.1.2.2. Case 1: ‘cut’ at Point D: all joint variables ....................................................................... 14
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
3.2. Dynamic Modeling (Lagrangian method) .................................................................................... 17
vi | P a g e
3.3. Gauss Divergence Theorem ....................................................................................................... 19
3.3.1. Theorem Statement ............................................................................................................. 19
3.3.2. Application to Workspace Volume ....................................................................................... 19
3.3.2.1. Example - 3-RPR mechanism ......................................................................................... 20
3.4. Simulink/Simmechanics .............................................................................................................. 21
3.5. MapleSim ................................................................................................................................... 21
4. Workspace Analysis ........................................................................................................................ 23
4.1. 3PRR ......................................................................................................................................... 24
4.1.1. Nomenclature ...................................................................................................................... 24
4.1.2. Parametric Sweeps with Inverse Position Kinematics .......................................................... 25
4.1.3. Recursive nearest neighbor search with Inverse Position Kinematics .................................. 26
4.1.4. Exact workspace determination using geometric constraints ............................................... 27
4.2. Hexapod ..................................................................................................................................... 31
4.2.1. Nomenclature ...................................................................................................................... 31
4.2.2. Parametric Sweeps with Inverse Position Kinematics .......................................................... 33
4.2.3. Parametric Sweeps with Geometric checks ......................................................................... 34
4.2.4. Recursive nearest neighbor search using Inverse Position Kinematics ............................... 36
4.2.5. Recursive nearest neighbor search using Geometric checks .............................................. 36
4.2.6. Exact Workspace determination using Geometric constraints ............................................. 38
4.2.7. Using CAD modeling software to compute volumes ............................................................ 40
4.3. Stewart Platform ......................................................................................................................... 43
5. Kinematic and Dynamic Analysis of 3-PRR ................................................................................... 44
5.1. Kinematic Analysis ..................................................................................................................... 44
vii | P a g e
5.2. Dynamic Analysis ....................................................................................................................... 47
5.2.1. Hand derivation ................................................................................................................... 47
5.2.2. Maple derivation .................................................................................................................. 48
5.2.3. MapleSim Modeling ............................................................................................................. 49
5.3. Matlab Kinematic Simulation ...................................................................................................... 51
5.3.1. Calculation of Initial Pose .................................................................................................... 51
5.3.2. Final Simulation Run ........................................................................................................... 51
5.4. Simmechanics Forward Dynamics Simulation Comparison ........................................................ 52
5.5. Simmechanics Control Dynamics Simulation .............................................................................. 54
6. Kinematic and Dynamic Analysis and Control of Hexapod .......................................................... 57
6.1. Kinematic Analysis ..................................................................................................................... 57
6.2. Dynamic Analysis ....................................................................................................................... 59
6.3. Forward Dynamics comparison between Matlab/Simmechanics/Maplesim ................................ 59
6.4. Control Dynamics Simulation with Simmechanics ...................................................................... 62
6.5. Further steps for Real-time implementation ................................................................................ 62
7. MapleSim for Education .................................................................................................................. 63
8. Future Scope ................................................................................................................................... 64
9. Conclusion ....................................................................................................................................... 64
10. References ................................................................................................................................... 64
11. Appendix ................................................................................................................................... 11-1
11.1. Parts and Specification ........................................................................................................ 11-1
11.2. 3-RPR Kinematics ............................................................................................................... 11-1
11.2.1. 9 joint variables set .......................................................................................................... 11-1
11.2.1.1. Positions and velocities of all points ........................................................................... 11-1
viii | P a g e
11.2.2. Set of 6 joint variables and 3 task variables ..................................................................... 11-2
11.3. Gauss Divergence Theorem Calculations ............................................................................ 11-3
11.3.1. Circular Arc ...................................................................................................................... 11-3
11.3.2. Line ................................................................................................................................. 11-3
11.4. Maple Code for Hexapod Dynamic Analysis ........................................................................ 11-4
11.5. 3PRR Dynamic Equations.................................................................................................... 11-7
11.5.1. Substitutions .................................................................................................................... 11-7
11.5.2. 7 variable set ................................................................................................................... 11-8
11.5.2.1. Force Vector .............................................................................................................. 11-8
11.5.2.2. Mass Matrix ............................................................................................................... 11-8
11.5.2.3. Constraint Matrix ........................................................................................................ 11-8
11.6. Hexapod Dynamic Equations ............................................................................................... 11-9
11.6.1. Substitutions .................................................................................................................... 11-9
11.6.2. 21 variable set ................................................................................................................. 11-9
11.6.2.1. Force Vector .............................................................................................................. 11-9
11.6.2.2. Mass Matrix ............................................................................................................. 11-10
11.6.2.3. Constraint Matrix ...................................................................................................... 11-11
11.6.2.4. Torque Dissipation Matrix ........................................................................................ 11-12
11.6.3. 24 variable set ............................................................................................................... 11-12
11.6.3.1. Force Vector ............................................................................................................ 11-12
11.6.3.2. Mass Matrix ............................................................................................................. 11-12
11.6.3.3. Constraint Matrix ...................................................................................................... 11-13
11.6.3.4. Torque Dissipation Matrix ........................................................................................ 11-13
ix | P a g e
11.6.4. 6 joint variables + 6 task variables set ........................................................................... 11-14
11.6.4.1. Constraint Matrix ...................................................................................................... 11-14
Table of Figures
Figure 1 - Hexapod ..................................................................................................................................... 4
Figure 2 - 3PRR and 3RPS Models ............................................................................................................ 6
Figure 3 - Hexapod model Simplification to 3-PRR ..................................................................................... 8
Figure 4 : 3RPR nomenclature ................................................................................................................. 13
Figure 5 - Constant Orientation Workspace of 3RPR (φ =0) ..................................................................... 20
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 8: Simplified 2-D 3PRR system ..................................................................................................... 24
Figure 9 - Workspace of 3PRR with grid size 0.01 .................................................................................... 26
Figure 10 : Recursive nearest neighbor search algorithm ......................................................................... 27
Figure 11 - Individual PRR workspaces in 3PRR ...................................................................................... 28
Figure 12 - Shifted individual PRR workspaces for 3PRR ......................................................................... 29
Figure 13 - Overlay of 3PRR individual workspaces with brute-force method workspace ......................... 29
Figure 14 - Workspace calculation using Gauss divergence - intersection Points ..................................... 30
Figure 15 - 3PRR constant orientation workspace for all orientations ....................................................... 31
Figure 16 - Hexapod Structure and Reference frames ............................................................................. 32
Figure 17 : Hexapod TLS ......................................................................................................................... 32
Figure 18 - Hexapod workspace for [0 0 0] orientation using brute force method with inverse position
kinematics routine .................................................................................................................. 33
x | P a g e
Figure 19 – Vector diagram for Geometry-based checks on Hexapod ...................................................... 34
Figure 20 – Geometric check method - rmin ............................................................................................... 35
Figure 21 - Geometric check method - rmax ............................................................................................... 35
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 25 – Exact Workspace using Geometric checks - 3D Workspace .................................................. 39
Figure 26 : Comparison of workspace from grid-search method (Shaded) and Exact workspace
method (Solid line) ................................................................................................................. 40
Figure 27 - 3D [0 0 0] orientation workspace by Exact Workspace method .............................................. 41
Figure 28 - Solidworks representation of constant orientation workspace of Hexapod at [0 0 0]
orientation .............................................................................................................................. 41
Figure 29 : Hexapod at Orientation [30 20 0] degrees .............................................................................. 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 32 : Workspace volume Optimization results from CAD model ...................................................... 43
Figure 33 - Comparison of Stewart Gough platform results with Yoshito paper [15] ................................. 43
Figure 34 : Maplesim model for Hexapod ................................................................................................. 50
Figure 35 - Initial Pose of 3PRR for dynamic Simulation ........................................................................... 52
Figure 36 - 3PRR Forward dynamics setup - Simmechanics .................................................................... 52
Figure 37 - 3PRR forward dynamics model - Simmechanics .................................................................... 53
Figure 38 - Maplesim/Matlab Forward dynamics comparison ................................................................... 54
Figure 39 - 3PRR Control dynamics setup - Simmechanics ..................................................................... 55
Figure 40 - Maplesim/Matlab control dynamics simulation ........................................................................ 56
Figure 41 : Kinematic Validation of 12 variable formulation of Hexapod ................................................... 58
xi | P a g e
Figure 42 - Hexapod Maplesim model ...................................................................................................... 59
Figure 43 : Overall model of Hexapod in Simmechanics ........................................................................... 60
Figure 44 : Forward dynamics model of Hexapod in Simmechanics ......................................................... 60
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) ..................................................... 61
Table of Equations
Equation 3-1 : Position constraint equation format .................................................................................... 12
Equation 3-2 : Velocity constraint equation format .................................................................................... 12
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-6 : 3RPR central platform position/orientation ......................................................................... 14
Equation 3-7 : 3RPR central platform velocities ........................................................................................ 14
Equation 3-8 : 3RPR Jacobian representation .......................................................................................... 14
Equation 3-9 : 3RPR, Case 1, variable sets .............................................................................................. 14
Equation 3-10 : 3RPR, Case 1, position constraints ................................................................................. 14
Equation 3-11 : 3RPR, Case 1, velocity constraints .................................................................................. 15
Equation 3-12 : 3RPR, Case 2, Variable sets ........................................................................................... 15
Equation 3-13 : 3RPR, Case 2, Position constraints ................................................................................. 15
Equation 3-14 : 3RPR, Case 2, Velocity constraints ................................................................................. 16
Equation 3-15 : 3RPR, Case 2, Variable sets ........................................................................................... 16
Equation 3-16 - Lagrangian formulation equation ..................................................................................... 17
Equation 3-17 : General dynamic equation for constrained systems ........................................................ 17
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-20 : Mathematical statement of Gauss divergence theorem ................................................... 19
xii | P a g e
Equation 3-21 : Mathematical statement for Gauss divergence Theorem (2D) ......................................... 19
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
orientation workspace ............................................................................................................ 39
Equation 5-1 : 3PRR – Position of link masses ......................................................................................... 44
Equation 5-2 : 3PRR - Position of Center of Platform ............................................................................... 44
Equation 5-3 : 3PRR – Velocities of all points in mechanism .................................................................... 45
Equation 5-4 : 3PRR – angle relationships ............................................................................................... 45
Equation 5-5 : 3PRR – Jacobian representation ....................................................................................... 45
Equation 5-6 - Overall kinematic Constraints ............................................................................................ 46
Equation 5-7 - DOF wise Constraints ....................................................................................................... 46
Equation 5-8 - Independent DOF calculation ............................................................................................ 46
Equation 5-9 - Null Space Component Calculation ................................................................................... 46
Equation 5-10 : Hand derivation of 3PRR dynamic EOMs ........................................................................ 47
Equation 5-11 : Dynamic matrices for the 3PRR (9 joint variables set) ..................................................... 48
Equation 5-12 - Reference frames and parameters .................................................................................. 49
Equation 13 : Forward dynamic simulation equation ................................................................................. 53
Equation 14 : Force projection method for Inverse dynamics ................................................................... 55
Equation 15 : Inverse dynamic simulation equation .................................................................................. 55
Equation 16 : Inverse dynamic simulation overall equation ....................................................................... 55
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
1 | 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
geometric parameters of the parallel manipulator.
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
aspects of parallel manipulators with special reference to the Hexapod:
• Workspace analysis
• Modeling, Equation of Motion (EOM) generation and simulation
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.
2 | P a g e
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
test them in simulations.
3 | P a g e
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
complexity of their analysis to a great extent.
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
and better inertial distribution among others.
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
positioning devices and fast packaging among others.
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
system resulting in restricted work space as compared to SCMs.
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
is a key issue in such applications.
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
4 | P a g e
system demands the use of different analytical techniques, numerical simulation tools, software
environments and computer-hardware interfaces, thus providing enough opportunities for practicing and
honing the skills learnt through the coursework.
1.1. Case Study: Hexapod Parallel Platform Manipulator
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.
5 | P a g e
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
kinematics routine in three ways:
• Improve each computation by replacing inverse position kinematics with a geometric check
routine
• Decrease number of computations by replacing exhaustive search with recursive nearest
neighbor search algorithm
• Eliminate grid search altogether by using a geometric constraint algorithm that finds the
exact curves bounding the workspace
• 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
Simmechanics and attempt to develop control simulations for the Hexapod.
• In an additional study, we also delve into the usefulness of Maplesim for augmenting learning in
basic robotics courses.
6 | P a g e
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.
Figure 2 - 3PRR and 3RPS Models
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-
7 | P a g e
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.
Next, we restrict three of its DOF so as to simplify it to resemble a 3-DOF 3-PRR.
We set the angle of the platform to [0,0,φ] and the position of the center of the platform to be [x,y,z0],
where z0 is a constant. The Hexapod thus simplifies as shown:
-2.5 -2 -1.5 -1 -0.5 0 0.5 1 1.5 2 2.5
-2
-1
0
1
2
3
0
1
2
X axis
Y axis
Z ax
is
8 | P a g e
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
manipulator of the Hexapod.
1.4. Organization of the rest of the Thesis
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.
9 | P a g e
2. Literature Review
2.1. Related work
2.1.1. Modeling and Analysis of parallel manipulators
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
their traditional counterparts.
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
and finally design.
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
measurement of the actuated joint space parameters.
One of the most common parallel platform manipulators, the Stewart-Gough platform, has been the
topic of much research [3-5] till date.
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
formulations and natural orthogonal complements.
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.
10 | P a g e
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 –
prismatic actuators and revolute actuators.
2.1.2. Workspace Analysis of parallel manipulators
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
automation in a mechanism design process.
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
11 | P a g e
ride. This prototype developed is somewhat similar to the Hexapod which is a Prismatic-Universal-Spherical
(PUS) 6-DOF-PM.
12 | P a g e
3. Mathematical and Technological Tools
3.1. Kinematic Analysis
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
states can depend on the choice of formulation.
For a general parallel manipulator with n states and m degrees of freedom (or independent states),
there would be n-m constraints.
So, the vector of n states, q, can be broken up into a vector with (n-m) dependent states, qdep and
another vector with m independent states, qind.
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
The Position constraints in the system, generally represented by C, are written as
𝐶𝐶 = 0
Equation 3-1 : Position constraint equation format
On differentiating the position constraints, the velocity constraints in the system are obtained and can
be written as
𝐴𝐴 ∙ ��𝑞 = 0
Equation 3-2 : Velocity constraint equation format
This equation can be decoupled into the independent and dependent states and can be rewritten as
𝐴𝐴𝑑𝑑𝑑𝑑𝑑𝑑 q𝑑𝑑𝑑𝑑𝑑𝑑 + 𝐴𝐴𝑖𝑖𝑖𝑖𝑑𝑑 q𝑖𝑖𝑖𝑖𝑑𝑑 = 0
Equation 3-3 : Velocity constraints in terms of dependent and independent variables
Hence, the dependent states can be expressed as a function of the independent states as
∴ q𝑑𝑑𝑑𝑑𝑑𝑑 = 𝐷𝐷 ∙ ��𝑞𝑖𝑖𝑖𝑖𝑑𝑑 , 𝐷𝐷 = −𝐴𝐴𝑑𝑑𝑑𝑑𝑑𝑑 −1 ∙ 𝐴𝐴𝑖𝑖𝑖𝑖𝑑𝑑
Equation 3-4 : Velocity relation between dependent and independent variables
13 | P a g e
On differentiating Equation 3-3, we get the acceleration level constraints
��𝐴 ∙ ��𝑞 + 𝐴𝐴 ∙ ��𝑞 = 0 → ��𝐴 ∙ ��𝑞 + 𝐴𝐴𝑑𝑑𝑑𝑑𝑑𝑑 ∙ ��𝑞𝑑𝑑𝑑𝑑𝑑𝑑 + 𝐴𝐴𝑖𝑖𝑖𝑖𝑑𝑑 ∙ ��𝑞𝑖𝑖𝑖𝑖𝑑𝑑 = 0
Equation 3-5 : Acceleration constraints between dependent and independent variables
Hence, the ��𝑞𝑑𝑑𝑑𝑑𝑑𝑑 can again be expressed in terms of ��𝑞𝑖𝑖𝑖𝑖𝑑𝑑 and ��𝑞.
3.1.2. Example: 3-RPR
3.1.2.1. Nomenclature
Figure 4 : 3RPR nomenclature
Bi : base revolute joint position for ith RPR subsystem
bxi : x-coordinate of base for ith RPR subsystem
byi : y-coordinate of base for ith RPR subsystem
Ci : second revolute joint connecting to the platform for ith RPR subsystem
D : end point of the center
si : prismatic joint length for ith RPR subsystem
li1 : length of first link for ith RPR subsystem
li2 : length of platform link for ith RPR subsystem
Hence, location and orientation of point D is expressed as
𝑋𝑋𝑑𝑑𝑖𝑖 = 𝑏𝑏𝑏𝑏𝑖𝑖 + (𝑙𝑙𝑖𝑖1 + 𝑠𝑠𝑖𝑖) ∗ cos(𝜃𝜃𝑖𝑖1) + 𝑙𝑙𝑖𝑖2 ∗ cos(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2)
𝑌𝑌𝑑𝑑𝑖𝑖 = 𝑏𝑏𝑏𝑏𝑖𝑖 + (𝑙𝑙𝑖𝑖1 + 𝑠𝑠𝑖𝑖) ∗ sin(𝜃𝜃𝑖𝑖1) + 𝑙𝑙𝑖𝑖2 ∗ sin(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2)
14 | P a g e
𝜃𝜃𝑖𝑖 = 𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2
Equation 3-6 : 3RPR central platform position/orientation
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 + ��𝜃𝑖𝑖2
Equation 3-7 : 3RPR central platform velocities
This can be rewritten as,
�𝑋𝑋��𝑑𝑖𝑖𝑌𝑌��𝑑𝑖𝑖��𝜃𝑖𝑖� = �
−(𝑙𝑙𝑖𝑖1 + 𝑠𝑠𝑖𝑖) ∗ sin(𝜃𝜃𝑖𝑖1)− 𝑙𝑙𝑖𝑖2 ∗ sin(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2) cos(𝜃𝜃𝑖𝑖1) −𝑙𝑙𝑖𝑖2 ∗ sin(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2)(𝑙𝑙𝑖𝑖1 + 𝑠𝑠𝑖𝑖) ∗ cos(𝜃𝜃𝑖𝑖1) + 𝑙𝑙𝑖𝑖2 ∗ cos(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2) 𝑠𝑠𝑖𝑖𝑖𝑖(𝜃𝜃𝑖𝑖1) 𝑙𝑙𝑖𝑖2 ∗ cos(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2)
1 0 1� ∙ �
��𝜃𝑖𝑖1��𝑠𝑖𝑖��𝜃𝑖𝑖2
�
𝑋𝑋��𝑋𝑖𝑖 = 𝐽𝐽𝑖𝑖 ∙ ��𝑞𝑖𝑖
Equation 3-8 : 3RPR Jacobian representation
3.1.2.2. Case 1: ‘cut’ at Point D: all joint variables
States, n=9
DOF, m=3 (independent states)
Constraints, n-m=6 (dependent states)
𝑞𝑞 = [𝜃𝜃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]
Equation 3-9 : 3RPR, Case 1, variable sets
𝐶𝐶 =
⎣⎢⎢⎢⎢⎡𝑋𝑋𝑑𝑑1 − 𝑋𝑋𝑑𝑑2𝑌𝑌𝑑𝑑1 − 𝑌𝑌𝑑𝑑2𝜃𝜃1 − 𝜃𝜃2𝑋𝑋𝑑𝑑1 − 𝑋𝑋𝑑𝑑3𝑌𝑌𝑑𝑑1 − 𝑌𝑌𝑑𝑑3𝜃𝜃1 − 𝜃𝜃3 ⎦
⎥⎥⎥⎥⎤
𝐶𝐶 =
⎣⎢⎢⎢⎢⎡𝑏𝑏𝑏𝑏1 + (𝑙𝑙11 + 𝑠𝑠1) ∗ cos(𝜃𝜃11) + 𝑙𝑙12 ∗ cos(𝜃𝜃11 + 𝜃𝜃12)− 𝑏𝑏𝑏𝑏2 − (𝑙𝑙21 + 𝑠𝑠2) ∗ cos(𝜃𝜃21) − 𝑙𝑙22 ∗ cos(𝜃𝜃21 + 𝜃𝜃22)𝑏𝑏𝑏𝑏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 ⎦⎥⎥⎥⎥⎤
Equation 3-10 : 3RPR, Case 1, position constraints
15 | P a g e
Hence, differentiating the position constraint matrix C results in
��𝐶 =
⎣⎢⎢⎢⎢⎢⎡𝑋𝑋��𝑑1 − 𝑋𝑋��𝑑2𝑌𝑌��𝑑1 − 𝑌𝑌��𝑑2��𝜃1 − ��𝜃2𝑋𝑋��𝑑1 − 𝑋𝑋𝑑𝑑3𝑌𝑌��𝑑1 − 𝑌𝑌��𝑑3��𝜃1 − ��𝜃3 ⎦
⎥⎥⎥⎥⎥⎤
= �𝐽𝐽1 ∙ ��𝑞1 − 𝐽𝐽2 ∙ ��𝑞2𝐽𝐽1 ∙ ��𝑞1 − 𝐽𝐽3 ∙ ��𝑞3
� = �𝐽𝐽1 −𝐽𝐽2 0𝐽𝐽1 0 −𝐽𝐽3
� ∙ ��𝑞
q𝑑𝑑𝑑𝑑𝑑𝑑 = q(4: 9), q𝑖𝑖𝑖𝑖𝑑𝑑 = q(1: 3)
𝐴𝐴𝑑𝑑𝑑𝑑𝑑𝑑 = 𝐴𝐴(: ,4: 9),𝐴𝐴𝑖𝑖𝑖𝑖𝑑𝑑 = 𝐴𝐴(: ,1: 3)
Equation 3-11 : 3RPR, Case 1, velocity constraints
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
other variables are set as dependent.
States, n=12
DOF, m=3 (independent states)
Constraints, n-m=9 (dependent states)
𝑞𝑞 = [𝑏𝑏𝑑𝑑,𝑏𝑏𝑑𝑑,𝜑𝜑𝑑𝑑,𝜃𝜃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]
Equation 3-12 : 3RPR, Case 2, Variable sets
𝐶𝐶 =
⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡
𝑏𝑏𝑑𝑑 − 𝑋𝑋𝑑𝑑1𝑏𝑏𝑑𝑑 − 𝑌𝑌𝑑𝑑1
𝜑𝜑𝑑𝑑 +5𝜋𝜋6− 𝜃𝜃1
𝑏𝑏𝑑𝑑 − 𝑋𝑋𝑑𝑑2𝑏𝑏𝑑𝑑 − 𝑌𝑌𝑑𝑑2
𝜑𝜑𝑑𝑑 +𝜋𝜋6− 𝜃𝜃2
𝑏𝑏𝑑𝑑 − 𝑋𝑋𝑑𝑑3𝑏𝑏𝑑𝑑 − 𝑌𝑌𝑑𝑑3
𝜑𝜑𝑑𝑑 −𝜋𝜋2− 𝜃𝜃3 ⎦
⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
Equation 3-13 : 3RPR, Case 2, Position constraints
𝐴𝐴 = �𝐼𝐼 −𝐽𝐽1 0 0𝐼𝐼 0 −𝐽𝐽2 0𝐼𝐼 0 0 −𝐽𝐽3
�
16 | P a g e
q𝑑𝑑𝑑𝑑𝑑𝑑 = q(4: 12), q𝑖𝑖𝑖𝑖𝑑𝑑 = q(1: 3)
𝐴𝐴𝑑𝑑𝑑𝑑𝑑𝑑 = 𝐴𝐴(: ,4: 12),𝐴𝐴𝑖𝑖𝑖𝑖𝑑𝑑 = 𝐴𝐴(: ,1: 3)
Equation 3-14 : 3RPR, Case 2, Velocity constraints
3.1.2.4. Case 3: ‘cut’ at Point C: 6 joint Variables and Task Variables
In this case, we express the points Ci in terms of x, y and φ in one representation and in terms of θi1
and si in the other representation.
States, n=9
DOF, m=3 (independent states)
Constraints, n-m=6 (dependent states)
𝑞𝑞 = [𝑏𝑏𝑑𝑑,𝑏𝑏𝑑𝑑,𝜑𝜑𝑑𝑑,𝜃𝜃11, 𝑠𝑠1,𝜃𝜃21, 𝑠𝑠2,𝜃𝜃31, 𝑠𝑠3]
��𝑞 = [𝑏𝑏��𝑑,𝑏𝑏��𝑑,𝜑𝜑��𝑑, ��𝜃11, ��𝑠1, ��𝜃21, ��𝑠2, ��𝜃31, ��𝑠3]
��𝑞𝑖𝑖𝑖𝑖𝑑𝑑 = [𝑏𝑏��𝑑,𝑏𝑏��𝑑,𝜑𝜑��𝑑]𝑎𝑎𝑖𝑖𝑑𝑑 ��𝑞𝑑𝑑𝑑𝑑𝑑𝑑 = [��𝜃11, ��𝑠1, ��𝜃21, ��𝑠2, ��𝜃31, ��𝑠3]
Equation 3-15 : 3RPR, Case 2, Variable sets
The coordinates of point Ci can be expressed in terms of joint space variables (Ciq) as well as in terms
of task space variables (Cix).
[𝐶𝐶1𝑏𝑏 𝐶𝐶2𝑏𝑏 𝐶𝐶3𝑏𝑏] = �cos(𝜑𝜑) − sin(𝜑𝜑) 𝑏𝑏𝑑𝑑sin(𝜑𝜑) cos(𝜑𝜑) 𝑏𝑏𝑑𝑑
0 0 1� ∙
⎣⎢⎢⎢⎡−
𝑙𝑙√3
𝑙𝑙2√3
𝑙𝑙2√3
0𝑙𝑙2
−𝑙𝑙2
1 1 1 ⎦⎥⎥⎥⎤
𝐶𝐶𝑖𝑖𝑞𝑞 = �𝑏𝑏𝑏𝑏𝑖𝑖 + (𝑙𝑙𝑖𝑖1 + 𝑠𝑠𝑖𝑖) ∗ cos(𝜃𝜃𝑖𝑖1)𝑏𝑏𝑏𝑏𝑖𝑖 + (𝑙𝑙𝑖𝑖1 + 𝑠𝑠𝑖𝑖) ∗ sin(𝜃𝜃𝑖𝑖1)
1�
On isolating the x and y coordinates and subtracting, we get a total of 6 constraints. The resulting
constraint matrix is
⎣⎢⎢⎢⎢⎢⎢⎡��𝐶1𝑏𝑏[1]− ��𝐶1𝑞𝑞[1]��𝐶1𝑏𝑏[2]− ��𝐶1𝑞𝑞[2]��𝐶2𝑏𝑏[1]− ��𝐶2𝑞𝑞[1]��𝐶2𝑏𝑏[2]− ��𝐶2𝑞𝑞[2]��𝐶3𝑏𝑏[1]− ��𝐶3𝑞𝑞[1]��𝐶3𝑏𝑏[2]− ��𝐶3𝑞𝑞[2]⎦
⎥⎥⎥⎥⎥⎥⎤
Where,
17 | P a g e
[��𝐶1𝑏𝑏 ��𝐶2𝑏𝑏 ��𝐶3𝑏𝑏] = �−sin(𝜑𝜑)��𝜑 −cos(𝜑𝜑)��𝜑 𝑏𝑏��𝑑cos(𝜑𝜑)��𝜑 −sin(𝜑𝜑)��𝜑 𝑏𝑏��𝑑
0 0 0� ∙
⎣⎢⎢⎢⎡−
𝑙𝑙√3
𝑙𝑙2√3
𝑙𝑙2√3
0𝑙𝑙2
−𝑙𝑙2
1 1 1 ⎦⎥⎥⎥⎤
��𝐶𝑖𝑖𝑞𝑞 = �−(𝑙𝑙𝑖𝑖1 + 𝑠𝑠𝑖𝑖) ∗ sin(𝜃𝜃𝑖𝑖1) ��𝜃𝑖𝑖1 + (��𝑠𝑖𝑖) ∗ cos(𝜃𝜃𝑖𝑖1)
(𝑙𝑙𝑖𝑖1 + 𝑠𝑠𝑖𝑖) ∗ cos(𝜃𝜃𝑖𝑖1)��𝜃𝑖𝑖1 + (��𝑠𝑖𝑖) ∗ sin(𝜃𝜃𝑖𝑖1)0
�
3.2. Dynamic Modeling (Lagrangian method)
Dynamic modeling is generally done using the Euler Lagrange method of derivation. The main
equation behind this approach is
𝜕𝜕𝜕𝜕𝜕𝜕�𝜕𝜕𝜕𝜕𝜕𝜕��𝑞� −
𝜕𝜕𝜕𝜕𝜕𝜕𝑞𝑞 =
𝜕𝜕𝜋𝜋𝜕𝜕��𝑞
Equation 3-16 - Lagrangian formulation equation
Where,
L = Lagrangian = T – V = (kinetic energy) – (potential energy)
t = time
q, q = state variables and their time derivatives
π = Total power input into the system
Next, we use the equation stated above for each of the state variables and assemble the resulting nine
equations in the standard form for expressing dynamic equations, i.e.
𝑴𝑴��𝒒 + 𝑽𝑽 = 𝑬𝑬𝑬𝑬 − 𝑨𝑨𝑻𝑻𝝀𝝀
Equation 3-17 : General dynamic equation for constrained systems
Where
M = Mass matrix
V = Force Vector
E = Power dissipation matrix
q = State variable space
𝑬𝑬 = Torque input
λ = Constraint forces
In this case,
18 | P a g e
𝑇𝑇𝑖𝑖 =12𝑚𝑚𝑖𝑖0���𝑏𝑖𝑖02 + ��𝑏𝑖𝑖02 � +
12𝑚𝑚𝑖𝑖1���𝑏𝑖𝑖12 + ��𝑏𝑖𝑖12 � +
12𝑚𝑚𝑖𝑖2���𝑏𝑖𝑖22 + ��𝑏𝑖𝑖22 � +
12𝐽𝐽𝑖𝑖1��𝜃𝑖𝑖12 +
12𝐽𝐽𝑖𝑖2���𝜃𝑖𝑖1 + ��𝜃𝑖𝑖2�
2
Vi = 0 (due to planar constraint and gravity perpendicular to plane)
𝜕𝜕𝑖𝑖 = 𝑇𝑇𝑖𝑖
𝜕𝜕 = �𝜕𝜕𝑖𝑖
3
𝑖𝑖=1
𝜋𝜋 = �𝜏𝜏𝑖𝑖 ∙ θi1
3
𝑖𝑖=1
Equation 3-18 : Dynamic terms related to EOM derivation for 3RPR
Where,
mi0 = mass of on first link ith RPR subsystem
mi1 = mass of slider on ith RPR subsystem
mi2 = mass of second link on ith RPR subsystem
Ji1 = moment of inertia of first link about z axis
Ji2 = moment of inertia of second link about z axis
To show the Euler-Lagrange formulation, only a part of the Lagrangian is shown so as to avoid long
equations and confusion.
𝜕𝜕𝑑𝑑𝑎𝑎𝑝𝑝𝜕𝜕 =m10l11
2 ��𝜃112
8+
12𝐽𝐽11��𝜃11
2
𝜋𝜋𝑑𝑑𝑎𝑎𝑝𝑝𝜕𝜕 = 𝜏𝜏1 ∙ ��𝜃11
𝜕𝜕𝜕𝜕𝑑𝑑𝑎𝑎𝑝𝑝𝜕𝜕𝜕𝜕��𝜃11
=m10l11
2 ��𝜃11
4+ 𝐽𝐽11��𝜃11
𝜕𝜕𝜕𝜕𝜕𝜕 �
𝜕𝜕𝜕𝜕𝑑𝑑𝑎𝑎𝑝𝑝𝜕𝜕𝜕𝜕��𝜃11
� =m10l11
2 ��𝜃11
4+ 𝐽𝐽11��𝜃11
𝜕𝜕𝜕𝜕𝑑𝑑𝑎𝑎𝑝𝑝𝜕𝜕𝜕𝜕𝜃𝜃11
= 0
𝑂𝑂𝑂𝑂𝑑𝑑𝑝𝑝𝑎𝑎𝑙𝑙𝑙𝑙 𝑑𝑑𝑎𝑎𝑝𝑝𝜕𝜕 𝑑𝑑𝑞𝑞𝑒𝑒𝑎𝑎𝜕𝜕𝑖𝑖𝑐𝑐𝑖𝑖 ∶ m10l11
2 ��𝜃11
4+ 𝐽𝐽11��𝜃11 = 𝜏𝜏1 ∙ ��𝜃11
Equation 3-19 : Euler Lagrange formulation for part of EOM of 3RPR
19 | P a g e
3.3. Gauss Divergence Theorem
3.3.1. Theorem Statement
Most simply put, the Gauss divergence theorem states that the sum of all sources minus the sum of all
sinks gives the net flow out of a region.
Formal statement: the outward flux of a vector field through a closed surface is equal to the volume
integral of the divergence on the region inside the surface.
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
� (∇.𝐅𝐅)dV𝑉𝑉
= � 𝑭𝑭.𝑑𝑑𝒂𝒂𝑑𝑑𝑉𝑉
Equation 3-20 : Mathematical statement of Gauss divergence theorem The theorem can also be used for a planar area A bounded by a curve S, in which case the equation
simplifies to
� (∇.𝐅𝐅)da𝑆𝑆
= � 𝑭𝑭.𝒏𝒏�𝑑𝑑𝑠𝑠𝑑𝑑𝑆𝑆
Equation 3-21 : Mathematical statement for Gauss divergence Theorem (2D) 3.3.2. Application to Workspace Volume
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
𝐹𝐹 =𝑏𝑏𝑖𝑖 + 𝑏𝑏𝑗𝑗
2⇒ ∇.𝐅𝐅 =
𝜕𝜕𝐹𝐹𝜕𝜕𝑏𝑏 ∙ ��𝑖+
𝜕𝜕𝐹𝐹𝜕𝜕𝑏𝑏 ∙ ��𝑗 =
12 +
12 = 1 ⇒ � (∇.𝐅𝐅)da
𝑆𝑆= � da
𝑆𝑆= 𝐴𝐴𝑝𝑝𝑑𝑑𝑎𝑎
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
theorem are covered in 11.3.
20 | 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-
effector while keeping the orientation of the central platform constant.
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
the constant orientation workspace (for φ=0) for the 3-RPR.
Figure 5 - Constant Orientation Workspace of 3RPR (φ =0)
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
by overlaying the output from the two methods.
-40 -30 -20 -10 0 10 20 30 40
-30
-20
-10
0
10
20
30
21 | P a g e
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
-40 -30 -20 -10 0 10 20 30 40
-30
-20
-10
0
10
20
30
22 | P a g e
used to validate the equations of motion generated by hand as well as validation of forward dynamic
simulation results.
In the analytical approach, we discuss the following staged solution process:
(1) Idealize the problem by making appropriate assumptions, such as its inertia is like that of an
idealized rod.
(2) Draw the free body diagram of the simplified model.
(3) Develop the appropriate governing equation of motions (EOM) using Newton’s laws of motion
as well as using Lagrangian method.
(4) Solve the EOM to obtain the desired solution.
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)
23 | 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
24 | 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.
Figure 8: Simplified 2-D 3PRR system
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 inertial reference frame.
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
links, is designated as (xe,ye).
The geometrical information of the simplified model is given below:
25 | P a g e
𝐵𝐵1𝐵𝐵2 = 𝐵𝐵2𝐵𝐵3 = 𝐵𝐵3𝐵𝐵1 = 𝜕𝜕
𝐵𝐵1𝑂𝑂 = 𝐵𝐵2𝑂𝑂 = 𝐵𝐵3𝑂𝑂 =𝜕𝜕√3
𝐶𝐶1𝐶𝐶2 = 𝐶𝐶2𝐶𝐶3 = 𝐶𝐶3𝐶𝐶1 = 𝑙𝑙
𝐶𝐶1𝑃𝑃 = 𝐶𝐶2𝑃𝑃 = 𝐶𝐶3𝑃𝑃 =𝑙𝑙√3
𝐴𝐴𝑖𝑖𝐵𝐵𝑖𝑖 = 𝑙𝑙𝑖𝑖1; 𝑖𝑖 = 1,2,3
The generalized state variable space is:
q = [s1, θ11, θ12, s2, θ21, θ22, s3, θ31, θ32]
The minimal actuated variable space is:
z = [s1, s2, s3]
The cartesian output variable space is:
X = [xe,ye,φ]
4.1.2. Parametric Sweeps with Inverse Position Kinematics
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,
then the point is considered to be a part of the workspace.
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.
26 | P a g e
The brute-force search using inverse position kinematics routine gives the following results:
Figure 9 - Workspace of 3PRR with grid size 0.01
In the above figure, the black dotted lines represent the lines of action of the prismatic joints.
4.1.3. Recursive nearest neighbor search with Inverse Position Kinematics
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
encountered in the sampling.
-3 -2 -1 0 1 2 3
2.5
3
3.5
4
4.5
5
5.5
6
6.5
7
7.5
27 | P a g e
Figure 10 : Recursive nearest neighbor search algorithm
4.1.4. Exact workspace determination using geometric constraints
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
for workspace area calculation from these workspace edges.
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
28 | 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.
Figure 11 - Individual PRR workspaces in 3PRR
In this figure, the thick dotted lines represent the prismatic joint motion limits and the thin solid lines
outside the figure represent the workspaces of the points Ci.
-5 -4 -3 -2 -1 0 1 2 3 4 5
-3
-2
-1
0
1
2
3
4
5
A1 B1
C1
A2
B2
C2
A3
B3
C3
P
29 | P a g e
Next, we just add the vector CiP beforehand in all the PRR linkages to shift the workspaces by certain
amounts. The resulting figure is shown below.
Figure 12 - Shifted individual PRR workspaces for 3PRR
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.
Figure 13 - Overlay of 3PRR individual workspaces with brute-force method workspace
-6 -4 -2 0 2 4 60
1
2
3
4
5
6
7
8
9
10
-8 -6 -4 -2 0 2 4 6 8 100
1
2
3
4
5
6
7
8
9
10
30 | P a g e
In the above figure, the dark patch is the densely populated constant orientation workspace computed
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.
Figure 14 - Workspace calculation using Gauss divergence - intersection Points
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
complete continuous constant orientation workspace of the 3PRR in question.
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
the common volume. One such workspace is shown below.
-6 -4 -2 0 2 4 60
1
2
3
4
5
6
7
8
9
10
31 | 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
center fixed [12].
4.2.1. Nomenclature
The Hexapod is simplified into a 6-PUS manipulator as shown below.
32 | P a g e
Figure 16 - Hexapod Structure and Reference frames
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
thick dotted lines). The end-effector reference frame is fixed at point P.
The Hexapod is considered to be a combination of 3 Two Link Subsystems (TLS) connected to a
central platform. One TLS is shown the figure below.
Figure 17 : Hexapod TLS
We choose a Hexapod with the following parameters for the workspace evaluation:
TABLE 1 : HEXAPOD AI REFERENCE FRAME POSITIONS
i αi(deg) [ bxi, byi, bzi] 1 0 [ 0,−𝜕𝜕 2√3, 0]⁄ 2 120 [ 𝜕𝜕/4, 𝜕𝜕 4√3, 0]⁄ 3 240 [−𝜕𝜕/4, 𝜕𝜕 4√3, 0]⁄
OA1
A2
A3 B
CP
OA1
B
CP
33 | 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,
then the point is considered to be a part of the workspace.
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]
combination and plotting them.
34 | 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:
𝑂𝑂𝑃𝑃����� + 𝑃𝑃𝐶𝐶����� = 𝑂𝑂𝐴𝐴����� + 𝐴𝐴𝐶𝐶�����
Equation 4-1 : Basic Vector loop closure Equation for Hexapod
where, PC - end points of platform relative to its center, OA
- location of local coordinate systems for
prismatic joints with respect to O, AC - the values we evaluate for this analysis. All the vectors are
expressed in the global reference frame O.
Figure 19 – Vector diagram for Geometry-based checks on Hexapod
Now we attempt to find the maximum and minimum values for the magnitude of AC using geometric
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
35 | 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 = 𝑙𝑙12 − (𝜕𝜕𝑚𝑚𝑎𝑎𝑏𝑏 − 𝑏𝑏𝑙𝑙𝑐𝑐𝑐𝑐𝑎𝑎𝑙𝑙 )2 + 𝑏𝑏𝑙𝑙𝑐𝑐𝑐𝑐𝑎𝑎𝑙𝑙2
𝑝𝑝𝑚𝑚𝑎𝑎𝑏𝑏2 = 𝑙𝑙12 − (𝜕𝜕𝑚𝑚𝑖𝑖𝑖𝑖 + 𝑏𝑏𝑙𝑙𝑐𝑐𝑐𝑐𝑎𝑎𝑙𝑙 )2 + 𝑏𝑏𝑙𝑙𝑐𝑐𝑐𝑐𝑎𝑎𝑙𝑙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
determine if the point in consideration is reachable by the corresponding TLS.
Figure 20 – Geometric check method - rmin
Figure 21 - Geometric check method - rmax
36 | 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
position kinematics for purposes of workspace determination.
4.2.4. Recursive nearest neighbor search using Inverse Position Kinematics
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-
structure to reveal the entire workspace.
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
the inverse position kinematics routine with a geometric check routine.
4.2.5. Recursive nearest neighbor search using Geometric checks
This method takes both suggestions 1 and 2 into consideration for achieving the best results among
the methods discussed so far.
It is found to be better than methods 2 and 3. However, the computation time for all the methods
discussed above increases drastically as the grid size is reduced.
37 | 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
estimate of the computation cost.
(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
Method bruteF InvPosKin
Nearest neighbor InvPosKin
bruteF GeomCheck
Nearest neighbor GeomCheck
Time 13139.78 610.31 91.88 66.78 bruteF_InvPosKin 13139.78 0 95.36 99.30 99.49 Nearest neighbor_InvPosKin 610.31 0 84.95 89.06 bruteF_GeomCheck 91.88 0 27.32 Nearest neighbor_GeomCheck 66.78 0
-2.5 -2 -1.5 -1-2
-1
0
1
2
3
4
5
log10(Grid size)
log1
0(Im
plem
enta
tion
Tim
e)
bruteF_InvPosKinbruteF_GeomCheckTree_InvPosKinTree_GeomCheck
-2.5 -2 -1.5 -1-5
-4.5
-4
-3.5
-3
-2.5
-2
-1.5
-1
log10(Grid size)
log1
0(%
Wor
kspa
ce V
olum
e E
rror
)
bruteF_InvPosKinbruteF_GeomCheckTree_InvPosKinTree_GeomCheck
38 | 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.
Given the vector loop closure for a single TLS:
𝑂𝑂𝑃𝑃����� + 𝑃𝑃𝐶𝐶����� = 𝑂𝑂𝐴𝐴����� + 𝐴𝐴𝐵𝐵����� + 𝐵𝐵𝐶𝐶�����
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:
𝑂𝑂𝑃𝑃����� = 𝑂𝑂𝐴𝐴����� + 𝐴𝐴𝐵𝐵����� + 𝐵𝐵𝐶𝐶����� − 𝑃𝑃𝐶𝐶�����
𝑂𝑂𝐴𝐴1�������� + 𝐴𝐴𝐵𝐵1������� + 𝐵𝐵𝐶𝐶1������� − 𝑃𝑃𝐶𝐶1������� = 𝑂𝑂𝐴𝐴2�������� + 𝐴𝐴𝐵𝐵2������� + 𝐵𝐵𝐶𝐶2������� − 𝑃𝑃𝐶𝐶2�������
Equation 4-4 : Rearranged vector loop closure equation for Geometry constraints modeling of Hexapod
As can be seen, all the vectors except 1 1BC
and 2 2B C
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
shows the original loop closure equation followed by Equation 4-4.
Figure 24 : Vector loop for Exact workspace using geometric constraints
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
39 | 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
limit constraints for three sliders are active simultaneously.
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
𝑖𝑖 = (12) ∗ (12 − 2) ∗ (12 − 2 − 2) = 960
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
resulting workspace envelope is obtained and is visually represented as in Figure 25.
Figure 25 – Exact Workspace using Geometric checks - 3D Workspace
40 | 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
to those found by using Exact Workspace method.
TABLE 3 : METHOD COMPARISON (ACCURACY)
Grid search method
(for different grid sizes)
Exact
workspace
method
Difference
0.1 (a) 0.05 (b) 0.025 (c) Exact (d) (d-b)
Min X -0.800 -0.850 -0.850 -0.875 -0.025
Min Y -0.900 -0.900 -0.900 -0.918 -0.018
Min Z 1.400 1.400 1.350 1.333 -0.017
Max X 0.800 0.800 0.800 0.797 -0.004
Max Y 0.700 0.750 0.750 0.775 0.025
Max Z 2.400 2.400 2.400 2.420 0.020
4.2.7. Using CAD modeling software to compute volumes
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!
41 | P a g e
Figure 27 - 3D [0 0 0] orientation workspace by Exact Workspace method
Figure 28 - Solidworks representation of constant orientation workspace of Hexapod at [0 0 0] orientation
As further validation of the method, the 3D constant orientation workspace of the Hexapod is shown for
an orientation of [30 20 0] degrees.
Figure 29 : Hexapod at Orientation [30 20 0] degrees
OA1
A2
A3 B
C
P
42 | 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- ([α, β, γ]T = [30, 20, 0])
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:
TABLE 4 - PARAMETER SWEEP VALUES FOR CAD MODEL
Parameter Axis Min Max Resolution # Values
LA-Platform Length X 1.0 3.0 0.1 21
R1- Link Length Y 2.3 4.3 0.1 21
Volume Z 0.4 1.26 - 441
43 | 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
plotted using MATLAB as shown in Figure 32.
Figure 32 : Workspace volume Optimization results from CAD model
4.3. Stewart Platform
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.
12
3 2 3 4 50
0.5
1
1.5
Length of all 6 LinksPlatform Length
Wor
kspa
ce V
olum
e
44 | P a g e
5. Kinematic and Dynamic Analysis of 3-PRR
The nomenclature of the 3PRR has been covered in 4.1.1.
5.1. Kinematic Analysis
As discussed, the various parameters are:
αi : Inclination of base link i in global reference frame. i=1:3
si : Position of prismatic joint i
θ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
L : Length of side of equi-triangular base
li1: Length of first revolute link in ith PRR Sub-System
li2: Length of second revolute link in ith PRR Sub-System
φ : Orientation of platform in the global reference frame
We attempt to find the locations of the centers of mass of each body and also the endpoint of each link.
Center of masses of prismatic mass, first link and second link
𝑏𝑏𝑖𝑖0 = 𝑏𝑏𝑏𝑏𝑖𝑖 + 𝑠𝑠𝑖𝑖 ∙ cos(𝛼𝛼𝑖𝑖)
𝑏𝑏𝑖𝑖0 = 𝑏𝑏𝑏𝑏𝑖𝑖 + 𝑠𝑠𝑖𝑖 ∙ 𝑠𝑠𝑖𝑖𝑖𝑖(𝛼𝛼𝑖𝑖)
𝑏𝑏𝑖𝑖1 = 𝑏𝑏𝑏𝑏𝑖𝑖 + 𝑠𝑠𝑖𝑖 ∙ cos(𝛼𝛼𝑖𝑖) +𝑙𝑙𝑖𝑖12∙ cos(𝜃𝜃𝑖𝑖1 + 𝛼𝛼𝑖𝑖)
𝑏𝑏𝑖𝑖1 = 𝑏𝑏𝑏𝑏𝑖𝑖 + 𝑠𝑠𝑖𝑖 ∙ 𝑠𝑠𝑖𝑖𝑖𝑖(𝛼𝛼𝑖𝑖) +𝑙𝑙𝑖𝑖12∙ sin(𝜃𝜃𝑖𝑖1 + 𝛼𝛼𝑖𝑖)
𝑏𝑏𝑖𝑖2 = 𝑏𝑏𝑏𝑏𝑖𝑖 + 𝑠𝑠𝑖𝑖 ∙ cos(𝛼𝛼𝑖𝑖) + 𝑙𝑙𝑖𝑖1 ∙ cos(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1) +𝑙𝑙𝑖𝑖22∙ cos(𝛼𝛼𝑖𝑖+𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2)
𝑏𝑏𝑖𝑖2 = 𝑏𝑏𝑏𝑏𝑖𝑖 + 𝑠𝑠𝑖𝑖 ∙ sin(𝛼𝛼𝑖𝑖) + 𝑙𝑙𝑖𝑖1 ∙ sin(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1) +𝑙𝑙𝑖𝑖22∙ sin(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2)
Equation 5-1 : 3PRR – Position of link masses
Center of platform:
𝑏𝑏𝑖𝑖3 = 𝑏𝑏𝑏𝑏𝑖𝑖 + 𝑠𝑠𝑖𝑖 ∙ cos(𝛼𝛼𝑖𝑖) + 𝑙𝑙𝑖𝑖1 ∙ cos(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1) + 𝑙𝑙𝑖𝑖2 ∙ cos(𝛼𝛼𝑖𝑖+𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2)
𝑏𝑏𝑖𝑖3 = 𝑏𝑏𝑏𝑏𝑖𝑖 + 𝑠𝑠𝑖𝑖 ∙ 𝑠𝑠𝑖𝑖𝑖𝑖(𝛼𝛼𝑖𝑖) + 𝑙𝑙𝑖𝑖1 ∙ sin(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1) + 𝑙𝑙𝑖𝑖2 ∙ sin(𝛼𝛼𝑖𝑖+𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2)
Equation 5-2 : 3PRR - Position of Center of Platform
45 | P a g e
Now, differentiating the position level equations, we get
𝑏𝑏𝑖𝑖0 = 𝑠𝑠��𝑖 ∙ cos(𝛼𝛼𝑖𝑖)
𝑏𝑏𝑖𝑖0 = 𝑠𝑠��𝑖 ∙ 𝑠𝑠𝑖𝑖𝑖𝑖(𝛼𝛼𝑖𝑖)
𝑏𝑏𝑖𝑖1 = 𝑠𝑠��𝑖 ∙ cos(𝛼𝛼𝑖𝑖) −𝑙𝑙𝑖𝑖12∙ sin(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1) ∙ 𝜃𝜃𝑖𝑖1
𝑏𝑏𝑖𝑖1 = 𝑠𝑠��𝑖 ∙ sin(𝛼𝛼𝑖𝑖) +𝑙𝑙𝑖𝑖12∙ cos(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1) ∙ 𝜃𝜃𝑖𝑖1
𝑏𝑏𝑖𝑖2 = 𝑠𝑠��𝑖 ∙ cos(𝛼𝛼𝑖𝑖) − 𝑙𝑙𝑖𝑖1 ∙ sin(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1) ∙ 𝜃𝜃𝑖𝑖1 −𝑙𝑙𝑖𝑖22∙ sin(𝛼𝛼𝑖𝑖+𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2) ∙ (𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 )
𝑏𝑏𝑖𝑖2 = 𝑠𝑠��𝑖 ∙ sin(𝛼𝛼𝑖𝑖) + 𝑙𝑙𝑖𝑖1 ∙ cos(𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1) ∙ 𝜃𝜃𝑖𝑖1 +𝑙𝑙𝑖𝑖22∙ cos(𝛼𝛼𝑖𝑖+𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2) ∙ (𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖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 )
Equation 5-3 : 3PRR – Velocities of all points in mechanism
However, we do not require the third differential equation because
𝛼𝛼𝑖𝑖 + 𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 = (𝑖𝑖 − 1) ∙2𝜋𝜋3
+𝜋𝜋6
+ 𝜑𝜑
∴ ��𝜃𝑖𝑖1 + ��𝜃𝑖𝑖2 = ��𝜑
Equation 5-4 : 3PRR – angle relationships
Hence, writing the equations in the state-space form:
���𝑏��𝑏��𝜑� = �
cos(𝛼𝛼𝑖𝑖) −𝑙𝑙𝑖𝑖1 ∙ sin(𝜃𝜃𝑖𝑖1) − 𝑙𝑙𝑖𝑖2 ∙ sin(𝜃𝜃𝑖𝑖2) −𝑙𝑙𝑖𝑖2 ∙ sin(𝜃𝜃𝑖𝑖2)sin(𝛼𝛼𝑖𝑖) 𝑙𝑙𝑖𝑖1 ∙ cos(𝜃𝜃𝑖𝑖1) + 𝑙𝑙𝑖𝑖2 ∙ cos(𝜃𝜃𝑖𝑖2) 𝑙𝑙𝑖𝑖2 ∙ cos(𝜃𝜃𝑖𝑖2)
0 1 1� �𝑠𝑠��𝑖𝜃𝜃𝑖𝑖1𝜃𝜃𝑖𝑖2
�
𝑋𝑋��𝑑 = 𝐽𝐽1𝑞𝑞1 ⇒ 𝑋𝑋��𝑑 = 𝐽𝐽2𝑞𝑞2 = 𝐽𝐽3𝑞𝑞3
Equation 5-5 : 3PRR – Jacobian representation
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
and 7 as the independent DOF’s.
46 | P a g e
In order to express the dependent DOF’s in terms of independent DOF’s, we rewrite the above
equations in the form:
�𝐽𝐽1q1 − 𝐽𝐽2q2𝐽𝐽1𝑞𝑞1 − 𝐽𝐽3𝑞𝑞3
�6∗1
= �00�6∗1
�𝐽𝐽1 −𝐽𝐽2 0𝐽𝐽1 0 −𝐽𝐽3
�6∗9
�𝑞𝑞1𝑞𝑞2𝑞𝑞3
�9∗1
= �00�6∗1
Equation 5-6 - Overall kinematic Constraints
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
independent degrees of freedom and find that,
𝐴𝐴𝑑𝑑𝑑𝑑𝑑𝑑 ∙
⎣⎢⎢⎢⎢⎢⎡𝜃𝜃11𝜃𝜃12𝜃𝜃21𝜃𝜃22𝜃𝜃31𝜃𝜃32 ⎦
⎥⎥⎥⎥⎥⎤
+ 𝐴𝐴𝑖𝑖𝑖𝑖𝑑𝑑 ∙ �𝑠𝑠1��𝑠2𝑠𝑠3
� = �00�6∗1
𝐴𝐴𝑑𝑑𝑑𝑑𝑑𝑑 ��𝑞𝑑𝑑𝑑𝑑𝑑𝑑 + 𝐴𝐴𝑖𝑖𝑖𝑖𝑑𝑑 ��𝑞𝑖𝑖𝑖𝑖𝑑𝑑 = 0
∴ ��𝑞𝑑𝑑𝑑𝑑𝑑𝑑 = 𝐶𝐶 ∙ ��𝑞𝑖𝑖𝑖𝑖𝑑𝑑 , 𝐶𝐶 = −𝐴𝐴𝑑𝑑𝑑𝑑𝑑𝑑 −1 ∙ 𝐴𝐴𝑖𝑖𝑖𝑖𝑑𝑑
Equation 5-7 - DOF wise Constraints
Now, we compute a matrix relating all DOF’s to the independent DOF’s
��𝑞𝑎𝑎𝑙𝑙𝑙𝑙 = 𝑆𝑆 ∙ ��𝑞𝑖𝑖𝑖𝑖𝑑𝑑
The final step is to compute the independent DOF’s in terms of the required end-effectors DOF’s. In
order to do that, we observe that,
𝑋𝑋��𝑑 = 𝐽𝐽1��𝑞1 𝑎𝑎𝑖𝑖𝑑𝑑 q1 = 𝑆𝑆(1: 3) ∙ ��𝑞𝑖𝑖𝑖𝑖𝑑𝑑 ⇒ 𝑞𝑞𝑖𝑖𝑖𝑖𝑑𝑑 = (𝐽𝐽1 ∙ 𝑆𝑆(1: 3))−1 ∙ 𝑋𝑋𝑑𝑑
Equation 5-8 - Independent DOF calculation
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.
The null-space matrix maybe calculated as
𝑁𝑁 = 𝐼𝐼 − (𝐽𝐽1 ∙ 𝑆𝑆(1: 3))−1 ∙ (𝐽𝐽1 ∙ 𝑆𝑆(1: 3))
Equation 5-9 - Null Space Component Calculation
Hence, given any state of the system, we can find the state derivatives.
47 | P a g e
5.2. Dynamic Analysis
5.2.1. Hand derivation
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
system, we define the Kinetic Energy for the ith subsystem as
𝑇𝑇𝑖𝑖 =12𝑚𝑚𝑖𝑖0���𝑏𝑖𝑖02 + ��𝑏𝑖𝑖02 � +
12𝑚𝑚𝑖𝑖1���𝑏𝑖𝑖12 + ��𝑏𝑖𝑖12 � +
12𝑚𝑚𝑖𝑖2���𝑏𝑖𝑖22 + ��𝑏𝑖𝑖22 � +
12𝐽𝐽𝑖𝑖1��𝜃𝑖𝑖12 +
12𝐽𝐽𝑖𝑖2���𝜃𝑖𝑖1 + ��𝜃𝑖𝑖2�
2
Vi = 0 (due to planar constraint and gravity perpendicular to plane)
𝜕𝜕𝑖𝑖 = 𝑇𝑇𝑖𝑖
𝜕𝜕 = �𝜕𝜕𝑖𝑖
3
𝑖𝑖=1
Equation 5-10 : Hand derivation of 3PRR dynamic EOMs
Where
mi0 = mass of slider on ith PRR subsystem
mi1 = mass of first link on ith PRR subsystem
mi2 = mass of second link on ith PRR subsystem
Ji1 = moment of inertia of first link about z axis
Ji2 = moment of inertia of second link about z axis
Here, state variables, q = [s1, θ11, θ12, s2, θ21, θ22, s3, θ31, θ32]
In this case,
𝑀𝑀 = �𝑀𝑀1 0 00 𝑀𝑀2 00 0 𝑀𝑀3
�
𝑉𝑉 = �𝑉𝑉1𝑉𝑉2𝑉𝑉3
�
48 | P a g e
𝑋𝑋 =
⎣⎢⎢⎢⎢⎢⎢⎢⎡1 0 00 0 00 0 00 1 00 0 00 0 00 0 10 0 00 0 0⎦
⎥⎥⎥⎥⎥⎥⎥⎤
𝑀𝑀𝑖𝑖
=
⎣⎢⎢⎢⎢⎡ mi0 + mi1 + mi2 −
12
mi1li1si1 − mi2li1si1 −12
mi2li2si3 −12
mi2li2si3
−12
mi1li1si1 − mi2li1si1 −12
mi2li2si314
mi1li12 + mi2li1
2 +14
mi2li22 + Ji1 + Ji2 + mi2li1li2ci2 Ji2 +
14
mi2li22 +
12
mi2li1li2ci2
−12
mi2li2si3 Ji2 +14
mi2li22 +
12
mi2li1li2ci214
mi2li22 + Ji2 ⎦
⎥⎥⎥⎥⎤
𝑉𝑉𝑖𝑖 =
⎣⎢⎢⎢⎢⎡12
mi1li1θi12 ci1 + mi2li1θi1
2 ci1 +12
mi2li2θ112 ci3 + mi2li2θi1θi2ci3 +
12
mi2li2θi22 ci3
12
mi2li1li2θi2si2(θi2 + 2θi1);
−12
mi2li1li2θi12 si2 ⎦
⎥⎥⎥⎥⎤
Equation 5-11 : Dynamic matrices for the 3PRR (9 joint variables set)
where
si1=sin(θi1); si2=sin(θi2); si3=sin(θi1+θi2)
ci1=cos(θi1); ci2=cos(θi2); ci3=cos(θi1+θi2)
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.
5.2.2. Maple derivation
Similar to hand derivation, we can do a symbolic analysis using Maple. The following are the basic
steps:
1. Create variable set and define them as functions of time.
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.
49 | 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.
A detailed description of Maple Code is available in 11.4.
On comparison of the txt files thus created, we find that the mass matrix, constraint matrix and force
vectors are a perfect term-by-term match.
5.2.3. MapleSim Modeling
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
via rotated reference frames. Gravity was in the negative z-direction.
• Inside each PRR system, we have a prismatic joint. Following that, we have two revolute joints
and links representing the 2R’s.
• The reference frames in the main are as follows:
Parameter Values MapleSim Parameter
Angles of rotation of base links in global
reference frame �0
2𝜋𝜋3
4𝜋𝜋3�
Rotation matrix angles for ref. frames
before PRR subsystems
Base locations for 3 prismatic joints
⎣⎢⎢⎡ 0
𝜕𝜕4
−𝜕𝜕4
−𝜕𝜕√3
𝜕𝜕2√3
−𝜕𝜕2√3⎦
⎥⎥⎤
Translation vectors for ref. frames before
PRR subsystems
Orientation of each PRR end link with
respect to second PRR end
30◦+(i-1)*120◦,
i=1:3
Rotation matrix angles for ref. frames after
PRR subsystems
Equation 5-12 - Reference frames and parameters
50 | P a g e
The resulting system is shown in the following figure:
Figure 34 : Maplesim model for Hexapod
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.
51 | P a g e
5.3. Matlab Kinematic Simulation
Before doing a dynamic simulation, a kinematic simulation was attempted. We use the constraint
matrix created from Maple derivation for this purpose.
5.3.1. Calculation of Initial Pose
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.
5.3.2. Final Simulation Run
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:
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
dotted black lines represent the base prismatic joints.
-2.5 -2 -1.5 -1 -0.5 0 0.5 1 1.5 2 2.5-1.5
-1
-0.5
0
0.5
1
1.5
2
2.5
3
52 | 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
Simmechanics to duplicate the MapleSim model.
Figure 35 - Initial Pose of 3PRR for dynamic Simulation
Figure 36 - 3PRR Forward dynamics setup - Simmechanics
-3 -2 -1 0 1 2 3-3
-2
-1
0
1
2
3
53 | 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
��𝐴 ∙ ��𝑞 + 𝐴𝐴 ∙ ��𝑞 = 0
we can write,
�𝑀𝑀 𝐴𝐴𝑇𝑇𝐴𝐴 0
� ���𝑞𝜆𝜆� = �𝑋𝑋𝜏𝜏 − 𝑉𝑉−��𝐴��𝑞 � ⇒ ���𝑞𝜆𝜆� = �𝑀𝑀 𝐴𝐴𝑇𝑇
𝐴𝐴 0�−1�𝑋𝑋𝜏𝜏 − 𝑉𝑉−��𝐴��𝑞 �
Equation 13 : Forward dynamic simulation equation
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
Matlab code is functional.
54 | 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.
(a) (b)
(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
5.5. Simmechanics Control Dynamics Simulation
In this step, we combine the inverse dynamics from Matlab with the forward dynamics from
Simmechanics to create a dynamic control simulation.
The control torque is determined by projecting onto the feasible motion space.
The EOM for a constrained manipulator are expressed as
0 5 10 15 20 25 30 35 40-1
0
1
2
3
4
5
6
7
s1dth11dth12ds2dth21dth22ds3dth31dth32d
0 5 10 15 20 25 30 35 40
-5
-4
-3
-2
-1
0
1
2
3
4x 10
-4
s1dth11dth12ds2dth21dth22ds3dth31dth32d
0 5 10 15 20 25 30 35 40-6
-4
-2
0
2
4
6x 10
-4
s1th11th12s2th21th22s3th31th32
0 5 10 15 20 25 30 35 40-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8
Tau1Tau2Tau3
55 | P a g e
𝑴𝑴��𝒒 + 𝑽𝑽 = 𝑬𝑬𝑬𝑬 − 𝑨𝑨𝑻𝑻𝝀𝝀
where, λ is the vector of constraint forces.
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-
multiply the entire equation with ST to get
𝑺𝑺𝑻𝑻𝑴𝑴��𝒒 + 𝑺𝑺𝑻𝑻𝑽𝑽 = 𝑺𝑺𝑻𝑻𝑬𝑬𝑬𝑬 − 𝑺𝑺𝑻𝑻𝑨𝑨𝑻𝑻𝝀𝝀 = 𝑺𝑺𝑻𝑻𝑬𝑬𝑬𝑬
(∵ 𝑨𝑨𝑺𝑺 = 𝟎𝟎 ⇒ 𝑺𝑺𝑻𝑻𝑨𝑨𝑻𝑻 = 𝟎𝟎)
Equation 14 : Force projection method for Inverse dynamics
Hence, the torque may be calculated as
𝑬𝑬 = (𝑺𝑺𝑻𝑻𝑬𝑬)(𝑺𝑺𝑻𝑻𝑴𝑴��𝒒 + 𝑺𝑺𝑻𝑻𝑽𝑽)
Equation 15 : Inverse dynamic simulation equation
In order to converge the initial errors in position/velocities (if any), we use PD control along with this
model-based control scheme.
𝑬𝑬 = (𝑺𝑺𝑻𝑻𝑬𝑬)−𝟏𝟏(𝑺𝑺𝑻𝑻𝑴𝑴��𝒒 + 𝑺𝑺𝑻𝑻𝑽𝑽) + 𝒌𝒌𝒑𝒑 ∙ (𝒛𝒛𝒅𝒅𝒅𝒅𝒅𝒅 − 𝒛𝒛𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂) + 𝒌𝒌𝒅𝒅 ∙ (��𝒛𝒅𝒅𝒅𝒅𝒅𝒅 − ��𝒛𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂𝒂)
Equation 16 : Inverse dynamic simulation overall equation
Where z represents the prismatic joint positions/velocities.
Figure 39 - 3PRR Control dynamics setup - Simmechanics
56 | P a g e
The motion profiles required are sine waves of different magnitudes and frequencies and the following
are the results for a 50 seconds simulation.
(a) (b)
(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
0 5 10 15 20 25 30 35 40 45 50-0.025
-0.02
-0.015
-0.01
-0.005
0
0.005
0.01
0.015
Pos_Error_s1
Pos_Error_s2Pos_Error_s3
0 5 10 15 20 25 30 35 40 45 50-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
Vel_Error_s1
Vel_Error_s2Vel_Error_s3
0 5 10 15 20 25 30 35 40 45 50-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8
Pos_s1
Pos_s2Pos_s3
0 5 10 15 20 25 30 35 40 45 50-25
-20
-15
-10
-5
0
5
10
15
Force_s1
Force_s2Force_s3
57 | P a g e
6. Kinematic and Dynamic Analysis and Control of Hexapod
The nomenclature of the Hexapod is covered in 4.2.1.
6.1. Kinematic Analysis
As discussed in kinematic analysis example in 3.1.2, there are various choices of variable spaces for
the kinematic and dynamic analysis of the hexapod.
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
imposed initial conditions.
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 dynamic matrices for this configuration can be found in 11.6.2.
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
prismatic displacements and platform orientation.
𝑞𝑞 = [𝑏𝑏𝑑𝑑,𝑏𝑏𝑑𝑑, 𝑎𝑎𝑑𝑑, 𝑠𝑠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 dynamic matrices can be found in 11.6.3.
The above formulation can also be used in the following configuration:
𝑞𝑞𝑑𝑑𝑑𝑑𝑑𝑑 = [𝑠𝑠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
along the legs. In this case,
58 | P a g e
𝑞𝑞 = [𝑠𝑠1, 𝑠𝑠2, 𝑠𝑠3, 𝑠𝑠4, 𝑠𝑠5, 𝑠𝑠6,𝑏𝑏𝑑𝑑,𝑏𝑏𝑑𝑑, 𝑎𝑎𝑑𝑑,𝑎𝑎𝑏𝑏,𝑎𝑎𝑏𝑏,𝑎𝑎𝑎𝑎]
𝑞𝑞𝑑𝑑𝑑𝑑𝑑𝑑 = [𝑠𝑠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
the 12-variable formulation and compare the results.
Figure 41 : Kinematic Validation of 12 variable formulation of Hexapod
(a) Path of end effector with constant orientation (b) Difference in prismatic joint positions between two separate kinematic runs (c) Actual prismatic joint positions from 12 variable set (d) Position Constraints from 12 variable set
-2 -1.5 -1 -0.5 0 0.5 1 1.5 2
-2
-1
0
1
2-1.5
-1
-0.5
0
0.5
1
1.5
Y axis
Z ax
is
0 1 2 3 4 5 6 7-5
-4
-3
-2
-1
0
1
2
3
4
5x 10
-5
Time (s)
Erro
r in
Pris
mat
ic J
oint
Pos
ition
s
s1s2s3s4s5s6
0 1 2 3 4 5 6 7-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
Time (s)
Prismatic Joints Positions
123456
0 1 2 3 4 5 6 7-3
-2.5
-2
-1.5
-1
-0.5
0
0.5x 10
-10
Time (s)
Pos
ition
Con
stra
ints
Position Constraints
123456
59 | 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
completed in future work.
6.2. Dynamic Analysis
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.
6.3. Forward Dynamics comparison between Matlab/Simmechanics/Maplesim
The Maplesim model created for the Hexapod is as shown below:
Figure 42 - Hexapod Maplesim model
This model is then run with some initial conditions and a fixed time step solver. This will allow for
comparison with simulation results from other software simulations.
The Simmechanics model for the Hexapod is shown below.
60 | P a g e
Figure 43 : Overall model of Hexapod in Simmechanics
Figure 44 : Forward dynamics model of Hexapod in Simmechanics
The mechanism fails with the slightest of gravity perturbations and is not analyzed further in this work.
Only comparison between Maplesim and Matlab will be possible.
A Maplesim simulation was run for 6.28 seconds with a fixed time step of 0.001.
61 | P a g e
The same simulation was run using Matlab code using the same initial conditions and a comparison of
the results is shown below.
(a) (b)
(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.
0 1 2 3 4 5 6 7-1.5
-1
-0.5
0
0.5
1
1.5x 10
-6 Prismatic Joints Position Comparison
123456
0 1 2 3 4 5 6 7-2
-1.5
-1
-0.5
0
0.5
1
1.5
2x 10
-6 Prismatic Joints Velocity Comparison
s1dots2dots3dots4dots5dots6dot
0 1 2 3 4 5 6
-2
-1.5
-1
-0.5
0
0.5
1
1.5
2
2.5
Prismatic Joints Positions
123456
0 1 2 3 4 5 6 7-1.5
-1
-0.5
0
0.5
1
1.5x 10
-5 Position Constraints
123456789101112131415
62 | 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.
6.5. Further steps for Real-time implementation
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.
63 | 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
course was limited because:
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
engineering experience from the user.
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
information obtained from the use of such software.
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
in different methods available to solve it.
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
mechanisms like the 3-PRR, 3-RPR etc.
A detailed description of the tutorials as well as their results is available in [19].
64 | 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
constant orientation workspace calculation was developed.
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
65 | 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.
11-1 | P a g e
11. Appendix
11.1. Parts and Specification
The significant parts in the Hexapod are listed below in the descending order of their characteristic
dimension.
Part name Nos.
Base platform 1
Hex table 1
Arm 6
Gimbal yoke upper 3
Gimbal yoke lower 3
Shaft support upper 3
Shaft support lower 3
Hex table BB 3
Arm BB 3
Gimbal cantilevers 30
11.2. 3-RPR Kinematics
11.2.1. 9 joint variables set
11.2.1.1. Positions and velocities of all points
Center of masses of first link, prismatic mass and second link
𝑏𝑏𝑖𝑖0 = 𝑏𝑏𝑏𝑏𝑖𝑖 +l𝑖𝑖12∙ cos(𝜃𝜃𝑖𝑖1)
𝑏𝑏𝑖𝑖0 = 𝑏𝑏𝑏𝑏𝑖𝑖 +l𝑖𝑖12∙ 𝑠𝑠𝑖𝑖𝑖𝑖(𝜃𝜃𝑖𝑖1)
𝑏𝑏𝑖𝑖1 = 𝑏𝑏𝑏𝑏𝑖𝑖 + (l𝑖𝑖1 + si) ∙ cos(𝜃𝜃𝑖𝑖1)
𝑏𝑏𝑖𝑖1 = 𝑏𝑏𝑏𝑏𝑖𝑖 + (l𝑖𝑖1 + si) ∙ 𝑠𝑠𝑖𝑖𝑖𝑖(𝜃𝜃𝑖𝑖1)
11-2 | P a g e
𝑏𝑏𝑖𝑖2 = 𝑏𝑏𝑏𝑏𝑖𝑖 + (l𝑖𝑖1 + si) ∙ cos(𝜃𝜃𝑖𝑖1) +𝑙𝑙𝑖𝑖22∙ cos(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2)
𝑏𝑏𝑖𝑖2 = 𝑏𝑏𝑏𝑏𝑖𝑖 + (l𝑖𝑖1 + si) ∙ sin(𝜃𝜃𝑖𝑖1) +𝑙𝑙𝑖𝑖22∙ sin(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2)
Center of platform:
𝑏𝑏𝑖𝑖3 = 𝑏𝑏𝑏𝑏𝑖𝑖 + (l𝑖𝑖1 + si) ∙ cos(𝜃𝜃𝑖𝑖1) + 𝑙𝑙𝑖𝑖2 ∙ cos(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2)
𝑏𝑏𝑖𝑖3 = 𝑏𝑏𝑏𝑏𝑖𝑖 + (l𝑖𝑖1 + si) ∙ sin(𝜃𝜃𝑖𝑖1) + 𝑙𝑙𝑖𝑖2 ∙ sin(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2)
Now, differentiating the position level equations, we get
𝑏𝑏𝑖𝑖0 = −l𝑖𝑖12∙ 𝑠𝑠𝑖𝑖𝑖𝑖(𝜃𝜃𝑖𝑖1)
𝑏𝑏𝑖𝑖0 =l𝑖𝑖12∙ cos(𝜃𝜃𝑖𝑖1)
𝑏𝑏𝑖𝑖1 = −(l𝑖𝑖1 + si) ∙ 𝑠𝑠𝑖𝑖𝑖𝑖(𝜃𝜃𝑖𝑖1) + si ∙ cos(𝜃𝜃𝑖𝑖1)
𝑏𝑏𝑖𝑖1 = (l𝑖𝑖1 + si) ∙ cos(𝜃𝜃𝑖𝑖1) + si ∙ sin(𝜃𝜃𝑖𝑖1)
𝑏𝑏𝑖𝑖2 = −(l𝑖𝑖1 + si) ∙ 𝑠𝑠𝑖𝑖𝑖𝑖(𝜃𝜃𝑖𝑖1) + si ∙ cos(𝜃𝜃𝑖𝑖1) −𝑙𝑙𝑖𝑖22∙ sin(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2) ∙ (𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 )
𝑏𝑏𝑖𝑖2 = (l𝑖𝑖1 + si) ∙ cos(𝜃𝜃𝑖𝑖1) + si ∙ sin(𝜃𝜃𝑖𝑖1) +𝑙𝑙𝑖𝑖22∙ cos(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2) ∙ (𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 )
𝑏𝑏𝑖𝑖3 = −(l𝑖𝑖1 + si) ∙ 𝑠𝑠𝑖𝑖𝑖𝑖(𝜃𝜃𝑖𝑖1) + si ∙ cos(𝜃𝜃𝑖𝑖1) − 𝑙𝑙𝑖𝑖2 ∙ sin(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2) ∙ (𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 )
𝑏𝑏𝑖𝑖3 = (l𝑖𝑖1 + si) ∙ cos(𝜃𝜃𝑖𝑖1) + si ∙ sin(𝜃𝜃𝑖𝑖1) + 𝑙𝑙𝑖𝑖2 ∙ cos(𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2) ∙ (𝜃𝜃𝑖𝑖1 + 𝜃𝜃𝑖𝑖2 )
The rest of the analysis is covered in the thesis.
11.2.2. Set of 6 joint variables and 3 task variables
xC=[-1,0,-1,0,-1,0;
0,-1,0,-1,0,-1;
-sin(φ)*l/sq3,cos(φ)*l/sq3,1/2*(sin(φ)*l+l*cos(φ)*sq3)/sq3,1/2*(-cos(φ)*l+l*sin(φ)*sq3)/sq3,1/2*(sin(φ)*l-
l*cos(φ)*sq3)/sq3,1/2*(-cos(φ)*l-l*sin(φ)*sq3)/sq3;
cos(th11),sin(th11),0,0,0,0;
-sin(th11)*s1-l11*sin(th11),cos(th11)*s1+l11*cos(th11),0,0,0,0;
0,0,cos(th21),sin(th21),0,0;
0,0,-l21*sin(th21)-sin(th21)*s2,l21*cos(th21)+cos(th21)*s2,0,0;
0,0,0,0,cos(th31),sin(th31);
0,0,0,0,-l31*sin(th31)-sin(th31)*s3,cos(th31)*s3+l31*cos(th31)]’;
11-3 | P a g e
11.3. Gauss Divergence Theorem Calculations
11.3.1. Circular Arc
A circular arc is represented by its center (xc,yc), its radius rc and the bounding angles θ1 and θ2.
Hence, at any given angle θ,
𝑖𝑖� = cos(𝜃𝜃) 𝑖𝑖 + sin(𝜃𝜃) 𝑗𝑗, 𝑏𝑏 = 𝑏𝑏𝑐𝑐 + 𝑝𝑝𝑐𝑐 ∙ cos(𝜃𝜃) , 𝑏𝑏 = 𝑏𝑏𝑐𝑐 + 𝑝𝑝𝑐𝑐 ∙ sin(𝜃𝜃) , 𝑑𝑑𝑠𝑠 = 𝑝𝑝𝑐𝑐 ∙ 𝑑𝑑𝜃𝜃
𝐹𝐹 ∙ 𝑖𝑖� =(𝑏𝑏𝑐𝑐 + 𝑝𝑝𝑐𝑐 ∙ cos(𝜃𝜃)) ∙ cos(𝜃𝜃) + (𝑏𝑏𝑐𝑐 + 𝑝𝑝𝑐𝑐 ∙ sin(𝜃𝜃)) ∙ sin(𝜃𝜃)
2=𝑏𝑏𝑐𝑐 ∙ cos(𝜃𝜃) + 𝑏𝑏𝑐𝑐 ∙ sin(𝜃𝜃) + 𝑝𝑝𝑐𝑐
2
� 𝑭𝑭.𝒏𝒏�𝑑𝑑𝑠𝑠𝑑𝑑𝑆𝑆
=12� (𝑏𝑏𝑐𝑐 ∙ cos(𝜃𝜃) + 𝑏𝑏𝑐𝑐 ∙ sin(𝜃𝜃) + 𝑝𝑝𝑐𝑐𝜃𝜃2
𝜃𝜃1
) ∙ 𝑝𝑝𝑐𝑐 ∙ 𝑑𝑑𝜃𝜃
=12 �𝑝𝑝𝑐𝑐 ∙ 𝑏𝑏𝑐𝑐 ∙ (sin(𝜃𝜃2)− sin(𝜃𝜃1))− 𝑝𝑝𝑐𝑐 ∙ 𝑏𝑏𝑐𝑐 ∙ (cos(𝜃𝜃2) − cos(𝜃𝜃1)) + 𝑝𝑝𝑐𝑐2 ∙ (𝜃𝜃2 − 𝜃𝜃1)�
Equation 11-1 : Area under circular arc using Gauss divergence theorem
11.3.2. Line
For a line going from point A (x1,y1) to point B (x2,y2).
𝑑𝑑 = �(𝑏𝑏2 − 𝑏𝑏1)2 + (𝑏𝑏2 − 𝑏𝑏1)2, 𝑖𝑖� = �𝑏𝑏1 − 𝑏𝑏2
𝑑𝑑� 𝑖𝑖 + (
y2 − y1d
)𝑗𝑗
𝑏𝑏 = 𝑏𝑏1 + 𝑏𝑏1 ∙ (𝑏𝑏2 − 𝑏𝑏1), 𝑏𝑏 = 𝑏𝑏1 + 𝑏𝑏1 ∙ (𝑏𝑏2 − 𝑏𝑏1)
𝐹𝐹 ∙ 𝑖𝑖� =�𝑏𝑏1 + 𝑏𝑏1 ∙ (𝑏𝑏1 − 𝑏𝑏2)� ∙ (𝑏𝑏1 − 𝑏𝑏2) + �𝑏𝑏1 + 𝑏𝑏1 ∙ (𝑏𝑏2 − 𝑏𝑏1)� ∙ (𝑏𝑏2 − 𝑏𝑏1)
2 ∙ 𝑑𝑑 =𝑏𝑏1
2 − 𝑏𝑏1 ∙ 𝑏𝑏2 + 𝑏𝑏1 ∙ 𝑏𝑏2 − 𝑏𝑏12 + 𝑏𝑏1 ∙ 𝑑𝑑2
2 ∙ 𝑑𝑑
� 𝑭𝑭.𝒏𝒏�𝑑𝑑𝑠𝑠𝑑𝑑𝑆𝑆
=1
2 ∙ 𝑑𝑑� �𝑏𝑏1
2 − 𝑏𝑏1 ∙ 𝑏𝑏2 + 𝑏𝑏1 ∙ 𝑏𝑏2 − 𝑏𝑏12 + 𝑏𝑏1 ∙ 𝑑𝑑2� ∙ 𝑑𝑑𝑏𝑏1
𝑏𝑏1𝑏𝑏
𝑏𝑏1𝑎𝑎
=�𝑏𝑏1
2 − 𝑏𝑏1 ∙ 𝑏𝑏2 + 𝑏𝑏1 ∙ 𝑏𝑏2 − 𝑏𝑏12� ∙ (𝑏𝑏1𝑏𝑏 − 𝑏𝑏1𝑎𝑎)
2 ∙ 𝑑𝑑+𝑑𝑑 ∙ (𝑏𝑏1𝑏𝑏2 − 𝑏𝑏1𝑎𝑎2)
4
Equation 11-2 : Area under line using Gauss divergence theorem
11-4 | 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-
effector reference frame.
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
Maplesim formulation of Hexapod EOM.
11-5 | 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-6 | 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
function ‘LagCalc’ to calculate the Lagrangian.
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-7 | 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
mass matrix and double-differential of all the states.
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.
This is done to get to the same result as Maplesim.
11.5. 3PRR Dynamic Equations
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-8 | P a g e
11.5.2. 7 variable set
11.5.2.1. Force Vector
vF=[m13*l12*th12d*th11d*c11*c12-
m13*l12*th12d*th11d*s11*s12+1/2*m13*l12*th12d^2*c11*c12+m13*l12*th12d^2*s11*s12*c43pi+1/2*m12*l11*th11d^2*c11+m13*l11*th11d^
2*c11+1/2*m13*l12*th11d^2*c11*c12+m13*l12*th11d^2*s11*s12*c43pi;
1/2*m13*l11*l12*th12d^2*s12+m13*l11*th11d*l12*s12*th12d;
m13*l11*th11d^2*l12*s12*c43pi+m33*l31*th31d^2*l32*s12*c43pi*c43pi+m33*l31*th31d^2*l32*c12*s43pi*c43pi;
1/2*m22*l21*th21d^2*c21+m23*l21*th21d^2*c21+1/2*th21d^2*m23*l22*c21*c22+th21d^2*m23*l22*s21*s22*c43pi;
0;
m33*l32*th12d*th31d*c31*c12*c43pi-m33*l32*th12d*th31d*c31*s12*s43pi-m33*l32*th12d*th31d*s31*s12*c43pi-
m33*l32*th12d*th31d*s31*c12*s43pi+1/2*m33*l32*th12d^2*c31*c12*c43pi+m33*l32*th12d^2*c31*s12*s43pi*c43pi+m33*l32*th12d^2*s31*
s12*c43pi*c43pi+m33*l32*th12d^2*s31*c12*s43pi*c43pi+1/2*m32*l31*th31d^2*c31+m33*l31*th31d^2*c31+1/2*m33*l32*th31d^2*c31*c12*
c43pi+m33*l32*th31d^2*c31*s12*s43pi*c43pi+m33*l32*th31d^2*s31*s12*c43pi*c43pi+m33*l32*th31d^2*s31*c12*s43pi*c43pi;
m33*l31*l32*th12d*s12*c43pi*th31d+1/2*m33*l31*l32*th12d^2*s12*c43pi+m33*l31*l32*th12d*c12*s43pi*th31d+1/2*m33*l31*l32*th1
2d^2*c12*s43pi];
11.5.2.2. Mass Matrix
xM=[-m11-m13-m12, 1/2*m12*l11*s11+m13*l11*s11+1/2*m13*l12*s11*c12+1/2*m13*l12*c11*s12,
1/2*m13*l12*s11*c12+1/2*m13*l12*c11*s12, 0, 0, 0, 0;
1/2*m12*l11*s11+m13*l11*s11+1/2*m13*l12*s11*c12+1/2*m13*l12*c11*s12, -1/4*m12*l11^2-m13*l11^2-1/4*m13*l12^2-J12-J13-
m13*l11*l12*c12, -1/2*m13*l11*l12*c12-J13-1/4*m13*l12^2, 0, 0, 0, 0;
1/2*m13*l12*s11*c12+1/2*m13*l12*c11*s12, -1/2*m13*l11*l12*c12-J13-1/4*m13*l12^2, -1/4*m13*l12^2-J13-1/4*m33*l32^2-J33, 0,
0, 1/2*m33*l32*s31*c12*c43pi-1/2*m33*l32*s31*s12*s43pi+1/2*m33*l32*c31*s12*c43pi+1/2*m33*l32*c31*c12*s43pi, -1/4*m33*l32^2-
J33-1/2*m33*l31*l32*c12*c43pi+1/2*m33*l31*l32*s12*s43pi;
0, 0, 0, -m21-m22-m23, 1/2*m22*l21*s21+m23*l21*s21+1/2*m23*l22*s21*c22+1/2*m23*l22*c21*s22, 0, 0;
0, 0, 0, 1/2*m22*l21*s21+m23*l21*s21+1/2*m23*l22*s21*c22+1/2*m23*l22*c21*s22, -1/4*m22*l21^2-m23*l21^2-1/4*m23*l22^2-
m23*l21*l22*c22-J22-J23, 0, 0;
0, 0, 1/2*m33*l32*s31*c12*c43pi-1/2*m33*l32*s31*s12*s43pi+1/2*m33*l32*c31*s12*c43pi+1/2*m33*l32*c31*c12*s43pi, 0, 0, -
m31-m32-m33, 1/2*m33*l32*s31*c12*c43pi-
1/2*m33*l32*s31*s12*s43pi+1/2*m33*l32*c31*s12*c43pi+1/2*m33*l32*c31*c12*s43pi+1/2*m32*l31*s31+m33*l31*s31;
0, 0, -1/4*m33*l32^2-J33-1/2*m33*l31*l32*c12*c43pi+1/2*m33*l31*l32*s12*s43pi, 0, 0, 1/2*m33*l32*s31*c12*c43pi-
1/2*m33*l32*s31*s12*s43pi+1/2*m33*l32*c31*s12*c43pi+1/2*m33*l32*c31*c12*s43pi+1/2*m32*l31*s31+m33*l31*s31, -1/4*m33*l32^2-
J33-m33*l31*l32*c12*c43pi+m33*l31*l32*s12*s43pi-1/4*m32*l31^2-m33*l31^2-J32];
11.5.2.3. Constraint Matrix
xC=[cal1, sal1, cal1, sal1;
-l11*sal1*c11-l11*cal1*s11-l12*sal1*c11*c12+l12*sal1*s11*s12-l12*cal1*s11*c12-l12*cal1*c11*s12, l11*cal1*c11-
l11*sal1*s11+l12*cal1*c11*c12-l12*cal1*s11*s12-l12*sal1*s11*c12-l12*sal1*c11*s12, -l11*sal1*c11-l11*cal1*s11-
l12*sal1*c11*c12+l12*sal1*s11*s12-l12*cal1*s11*c12-l12*cal1*c11*s12, l11*cal1*c11-l11*sal1*s11+l12*cal1*c11*c12-
l12*cal1*s11*s12-l12*sal1*s11*c12-l12*sal1*c11*s12;
-l12*sal1*c11*c12+l12*sal1*s11*s12-l12*cal1*s11*c12-l12*cal1*c11*s12, l12*cal1*c11*c12-l12*cal1*s11*s12-l12*sal1*s11*c12-
l12*sal1*c11*s12, -l12*sal1*c11*c12+l12*sal1*s11*s12-l12*cal1*s11*c12-l12*cal1*c11*s12+l32*sal3*c31*c12*c43pi-
l32*sal3*c31*s12*s43pi-l32*sal3*s31*s12*c43pi-l32*sal3*s31*c12*s43pi+l32*cal3*s31*c12*c43pi-
l32*cal3*s31*s12*s43pi+l32*cal3*c31*s12*c43pi+l32*cal3*c31*c12*s43pi, l12*cal1*c11*c12-l12*cal1*s11*s12-l12*sal1*s11*c12-
l12*sal1*c11*s12-
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;
-cal2, -sal2, 0, 0;
l21*sal2*c21+l21*cal2*s21+l22*sal2*c21*c22-l22*sal2*s21*s22+l22*cal2*s21*c22+l22*cal2*c21*s22, -21*cal2*c21+l21*sal2*s21-
l22*cal2*c21*c22+l22*cal2*s21*s22+l22*sal2*s21*c22+l22*sal2*c21*s22, 0, 0;
0, 0, -cal3, -sal3;
0, 0, l31*sal3*c31+l31*cal3*s31+l32*sal3*c31*c12*c43pi-l32*sal3*c31*s12*s43pi-l32*sal3*s31*s12*c43pi-
l32*sal3*s31*c12*s43pi+l32*cal3*s31*c12*c43pi-l32*cal3*s31*s12*s43pi+l32*cal3*c31*s12*c43pi+l32*cal3*c31*c12*s43pi, -
l31*cal3*c31+l31*sal3*s31-
11-9 | 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. Hexapod Dynamic Equations
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.6.2. 21 variable set
11.6.2.1. Force Vector
vF=[1/2*m1*l1*bd1^2*sb1;
2/3*M*l*sq3*axd*sal1*sax*saz*azd+2/3*M*ayd*l*sq3*cal1*say*caz*azd-
2/3*M*l*sq3*axd*sal1*saz*cax*cay*ayd+1/3*M*l*sq3*axd^2*sax*say*saz*sal1-2/3*M*l*sq3*axd*sal1*cax*say*caz*azd-
1/3*M*l*sq3*axd^2*cax*caz*sal1-
1/3*M*azd^2*l*sq3*cax*caz*sal1+1/3*M*ayd^2*l*sq3*cay*saz*cal1+1/3*M*azd^2*l*sq3*cay*saz*cal1+1/3*M*ayd^2*l*sq3*sax*say*saz*s
al1-2/3*M*ayd*l*sq3*sal1*caz*sax*cay*azd+1/2*m2*l2*bd2^2*sb2+M*l2*bd2^2*sb2+1/3*M*azd^2*l*sq3*sax*say*saz*sal1;
1/2*m3*l3*bd3^2*sb3;
1/2*m4*l4*bd4^2*sb4;
1/2*m5*l5*bd5^2*sb5;
1/2*m6*l6*bd6^2*sb6;
-JX*ayd*cay*azd+JY*ayd*cay*azd+JZ*azd*cay*ayd+2/3*M*l^2*axd*ayd*say*cay-
1/3*M*l^2*ayd^2*say*saz*caz+1/3*M*g*l*sq3*cax*caz+2/3*M*l^2*ayd*cay*caz^2*azd-2*JY*axd*say*cay*ayd+2*JZ*axd*ayd*say*cay-
JX*ayd^2*say*saz*caz+JY*ayd^2*say*saz*caz+2*JX*ayd*cay*caz^2*azd-2*JY*ayd*cay*caz^2*azd-1/3*M*g*l*sq3*sax*say*saz-
2/3*M*l^2*axd*say*cay*caz^2*ayd-2/3*M*l^2*axd*cay^2*saz*caz*azd-1/3*M*l2*bd2^2*l*sq3*sal1*sb2*sax*caz-
2/3*M*l2*ad2*l*sq3*sb2*saz*cax*sa2*say*cal1*bd2-
2/3*M*l2*ad2*l*sq3*sb2*sax*caz*cal1*sa2*bd2+1/3*M*l2*ad2^2*l*sq3*cb2*saz*cax*ca2*say*cal1+1/3*M*l2*ad2^2*l*sq3*cb2*sax*caz*c
al1*ca2+1/3*M*l2*bd2^2*l*sq3*cb2*sax*caz*cal1*ca2+2/3*M*l2*bd2*l*sq3*ca2*sb2*sax*say*saz*ad2-
2/3*M*l2*bd2*l*sq3*ca2*sb2*cax*caz*ad2+1/3*M*l2*ad2^2*l*sq3*sa2*cb2*sax*say*saz+1/3*M*l2*bd2^2*l*sq3*sa2*cb2*sax*say*saz-
1/3*M*l2*bd2^2*l*sq3*sal1*sb2*cax*say*saz-1/3*M*l2*ad2^2*l*sq3*sa2*cb2*cax*caz-
1/3*M*l2*bd2^2*l*sq3*sa2*cb2*cax*caz+1/3*M*l2*bd2^2*l*sq3*cb2*saz*cax*ca2*say*cal1-
2*JX*axd*say*cay*caz^2*ayd+2*JY*axd*say*cay*caz^2*ayd-2*JX*axd*cay^2*saz*caz*azd+2*JY*axd*cay^2*saz*caz*azd;
JY*axd^2*say*cay-JZ*axd^2*say*cay-JX*axd*cay*azd+JY*axd*cay*azd-
JZ*azd*cay*axd+2/3*M*ayd*l^2*azd*saz*caz+2/3*M*l^2*axd*cay*caz^2*azd+1/3*M*l^2*axd^2*say*cay*caz^2-2/3*M*l^2*axd*azd*cay-
1/3*M*l^2*axd^2*say*cay+2*JX*ayd*azd*saz*caz-2*JY*ayd*azd*saz*caz+JX*axd^2*say*cay*caz^2-
JY*axd^2*say*cay*caz^2+2*JX*axd*cay*caz^2*azd-
2*JY*axd*cay*caz^2*azd+1/3*M*g*l*sq3*saz*cax*cay+1/3*M*l2*bd2^2*l*sq3*cal1*sb2*say*saz-
1/3*M*l2*ad2^2*l*sq3*sa2*cb2*saz*cax*cay-1/3*M*l2*bd2^2*l*sq3*sa2*cb2*saz*cax*cay-
2/3*M*l2*ad2*l*sq3*sb2*say*saz*sal1*sa2*bd2-1/3*M*l2*bd2^2*l*sq3*sal1*sb2*saz*sax*cay-
2/3*M*l2*ad2*l*sq3*sb2*saz*sax*cay*cal1*sa2*bd2+1/3*M*l2*bd2^2*l*sq3*cb2*saz*sax*ca2*cay*cal1+1/3*M*l2*ad2^2*l*sq3*cb2*saz*s
ax*ca2*cay*cal1+1/3*M*l2*bd2^2*l*sq3*cb2*say*saz*sal1*ca2+1/3*M*l2*ad2^2*l*sq3*cb2*say*saz*sal1*ca2-
2/3*M*l2*bd2*l*sq3*ca2*sb2*saz*cax*cay*ad2;
JX*axd*ayd*cay-JY*axd*ayd*cay+JZ*cay*ayd*axd-JX*ayd^2*saz*caz+JY*ayd^2*saz*caz-1/3*M*g*l*sq3*sax*saz-
2/3*M*l^2*axd*ayd*cay*caz^2+1/3*M*l^2*axd^2*cay^2*saz*caz+2/3*M*l^2*axd*cay*ayd-
1/3*M*ayd^2*l^2*saz*caz+JX*axd^2*cay^2*saz*caz-JY*axd^2*cay^2*saz*caz-
2*JX*axd*ayd*cay*caz^2+2*JY*axd*ayd*cay*caz^2+1/3*M*g*l*sq3*cax*say*caz-1/3*M*l2*bd2^2*l*sq3*sal1*sb2*sax*say*caz-
1/3*M*l2*bd2^2*l*sq3*cal1*sb2*cay*caz+2/3*M*l2*ad2*l*sq3*sb2*cay*caz*sal1*sa2*bd2-1/3*M*l2*ad2^2*l*sq3*sa2*cb2*cax*say*caz-
1/3*M*l2*bd2^2*l*sq3*sa2*cb2*cax*say*caz+1/3*M*l2*bd2^2*l*sq3*sa2*cb2*sax*saz+1/3*M*l2*ad2^2*l*sq3*sa2*cb2*sax*saz-
11-10 | 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.6.2.2. Mass Matrix
xM=[m1, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/2*m1*l1*cb1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, m2+M, 0, 0, 0, 0, -1/3*M*l*sq3*sal1*sax*caz-1/3*M*l*sq3*sal1*cax*say*saz, 1/3*M*l*sq3*cal1*say*saz-
1/3*M*l*sq3*sal1*saz*sax*cay, -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, -
1/2*m2*l2*cb2-M*l2*cb2, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, m3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/2*m3*l3*cb3, 0, 0, 0, 0, 0, 0;
0, 0, 0, m4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/2*m4*l4*cb4, 0, 0, 0, 0;
0, 0, 0, 0, m5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/2*m5*l5*cb5, 0, 0;
0, 0, 0, 0, 0, m6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/2*m6*l6*cb6;
0, -1/3*M*l*sq3*sal1*sax*caz-1/3*M*l*sq3*sal1*cax*say*saz, 0, 0, 0, 0, JX*cay^2*caz^2-1/3*M*l^2*cay^2-
JY*cay^2*caz^2+1/3*M*l^2*cay^2*caz^2+JZ+1/3*M*l^2+JY*cay^2-JZ*cay^2, JX*cay*saz*caz-JY*cay*saz*caz+1/3*M*l^2*cay*saz*caz,
1/3*M*l^2*say+JZ*say, 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*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, 0, 0, 0, 0, 0, 0, 0, 0;
0, 1/3*M*l*sq3*cal1*say*saz-1/3*M*l*sq3*sal1*saz*sax*cay, 0, 0, 0, 0, JX*cay*saz*caz-
JY*cay*saz*caz+1/3*M*l^2*cay*saz*caz, JX-1/3*M*l^2*caz^2-JX*caz^2+JY*caz^2+1/3*M*l^2, 0, 0, 0,
11-11 | 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.6.2.3. Constraint Matrix
xC=[0, 0, 0, 0, 0, 0, -cal1, -sal1, 0, 0, 0, 0, 0, 0, 0;
-cal1, -sal1, 0, -cal1, -sal1, 0, cal1, sal1, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, -cal2, -sal2, 0, 0, 0, 0;
cal2, sal2, 0, 0, 0, 0, 0, 0, 0, cal2, sal2, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -cal3, -sal3, 0;
0, 0, 0, cal3, sal3, 0, 0, 0, 0, 0, 0, 0, cal3, sal3, 0;
0, -1/2*l*(cax*say*caz-sax*saz-sq3*cax*say*saz-sq3*sax*caz), -1/2*l*(sax*say*caz+cax*saz-sq3*sax*say*saz+sq3*cax*caz), 0,
1/2*l*(cax*say*caz-sax*saz+sq3*cax*say*saz+sq3*sax*caz), -1/2*l*(-sax*say*caz-cax*saz-sq3*sax*say*saz+sq3*cax*caz), 0, 0, 0,
0, 0, 0, 0, 0, 0;
1/2*say*l*(caz-sq3*saz), -1/2*l*sax*cay*(caz-sq3*saz), 1/2*l*cax*cay*(caz-sq3*saz), -1/2*say*l*(caz+sq3*saz),
1/2*l*sax*cay*(caz+sq3*saz), -1/2*l*cax*cay*(caz+sq3*saz), 0, 0, 0, 0, 0, 0, 0, 0, 0;
1/2*cay*l*(saz+sq3*caz), -1/2*l*(-sax*say*saz+cax*caz-sq3*sax*say*caz-sq3*cax*saz), -
1/2*l*(cax*say*saz+sax*caz+sq3*cax*say*caz-sq3*sax*saz), 1/2*cay*l*(-saz+sq3*caz), 1/2*l*(-
sax*say*saz+cax*caz+sq3*sax*say*caz+sq3*cax*saz), -1/2*l*(-cax*say*saz-sax*caz+sq3*cax*say*caz-sq3*sax*saz), 0, 0, 0, 0, 0,
0, 0, 0, 0;
0, 0, 0, 0, 0, 0, -l1*sal1*sa1*cb1, l1*cal1*sa1*cb1, -ca1*cb1*l1, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, -l1*(-cal1*cb1+sal1*ca1*sb1), l1*(sal1*cb1+cal1*ca1*sb1), sa1*sb1*l1, 0, 0, 0, 0, 0, 0;
-l2*sal1*sa2*cb2, l2*cal1*sa2*cb2, -ca2*cb2*l2, -l2*sal1*sa2*cb2, l2*cal1*sa2*cb2, -ca2*cb2*l2, l2*sal1*sa2*cb2, -
l2*cal1*sa2*cb2, ca2*cb2*l2, 0, 0, 0, 0, 0, 0;
-l2*(-cal1*cb2+sal1*ca2*sb2), l2*(sal1*cb2+cal1*ca2*sb2), sa2*sb2*l2, -l2*(-cal1*cb2+sal1*ca2*sb2),
l2*(sal1*cb2+cal1*ca2*sb2), sa2*sb2*l2, l2*(-cal1*cb2+sal1*ca2*sb2), -l2*(sal1*cb2+cal1*ca2*sb2), -sa2*sb2*l2, 0, 0, 0, 0,
0, 0;
11-12 | 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.6.2.4. Torque Dissipation Matrix
E=[eye(6), zeros(15, 6)];
11.6.3. 24 variable set
11.6.3.1. Force Vector
vF=[0;
0;
M*g;
-1/2*m1*l1*cb1*bd1^2;
-1/2*m2*l2*cb2*bd2^2;
-1/2*m3*l3*bd3^2*cb3;
-1/2*m4*l4*bd4^2*cb4;
-1/2*m5*l5*bd5^2*cb5;
-1/2*m6*l6*bd6^2*cb6;
0;
0;
0;
1/2*m1*l1^2*ad1*bd1*sb1*cb1-2*ad1*Jy*bd1*sb1*cb1+2*ad1*Jz*bd1*sb1*cb1+1/2*m1*g*l1*ca1*sb1;
-1/4*m1*l1^2*ad1^2*sb1*cb1+1/2*m1*g*l1*cb1*sa1+ad1^2*Jy*sb1*cb1-ad1^2*Jz*sb1*cb1;
1/2*m2*l2^2*ad2*bd2*sb2*cb2-2*ad2*Jy*bd2*sb2*cb2+2*ad2*Jz*bd2*sb2*cb2+1/2*m2*g*l2*ca2*sb2;
1/2*m2*g*l2*sa2*cb2-1/4*m2*l2^2*ad2^2*sb2*cb2+ad2^2*Jy*sb2*cb2-ad2^2*Jz*sb2*cb2;
1/2*m3*l3^2*ad3*bd3*sb3*cb3-2*ad3*Jy*bd3*sb3*cb3+2*ad3*Jz*bd3*sb3*cb3+1/2*m3*g*l3*ca3*sb3;
ad3^2*Jy*sb3*cb3-ad3^2*Jz*sb3*cb3+1/2*m3*g*l3*sa3*cb3-1/4*m3*l3^2*ad3^2*sb3*cb3;
2*ad4*Jz*bd4*sb4*cb4-2*ad4*Jy*bd4*sb4*cb4+1/2*m4*l4^2*ad4*bd4*sb4*cb4+1/2*m4*g*l4*ca4*sb4;
-ad4^2*Jz*sb4*cb4+ad4^2*Jy*sb4*cb4+1/2*m4*g*l4*sa4*cb4-1/4*m4*l4^2*ad4^2*sb4*cb4;
-2*ad5*Jy*bd5*sb5*cb5+2*ad5*Jz*bd5*sb5*cb5+1/2*m5*l5^2*ad5*bd5*sb5*cb5+1/2*m5*g*l5*ca5*sb5;
1/2*m5*g*l5*sa5*cb5-1/4*m5*l5^2*ad5^2*sb5*cb5+ad5^2*Jy*sb5*cb5-ad5^2*Jz*sb5*cb5;
1/2*m6*l6^2*ad6*bd6*sb6*cb6-2*ad6*Jy*bd6*sb6*cb6+2*ad6*Jz*bd6*sb6*cb6+1/2*m6*g*l6*ca6*sb6;
1/2*m6*g*l6*sa6*cb6-1/4*m6*l6^2*ad6^2*sb6*cb6+ad6^2*Jy*sb6*cb6-ad6^2*Jz*sb6*cb6];
11.6.3.2. Mass Matrix
xM=[M, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, M, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, M, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, m1, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/2*m1*l1*sb1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, m2, 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, 0, 0, 0, m3, 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, 0, m4, 0, 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, m5, 0, 0, 0, 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, m6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/2*m6*l6*sb6;
0, 0, 0, 0, 0, 0, 0, 0, 0, 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, JY, 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, 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*m1*l1^2*cb1^2+1/4*m1*l1^2+Jy*cb1^2-Jz*cb1^2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
11-13 | 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.6.3.3. Constraint Matrix
xC=[0, 0, 0, 0, 0, 0, 0, 0, 0, -1, 0, 0, -1, 0, 0, -1, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1, 0, 0, -1, 0, 0, -1, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1, 0, 0, -1, 0, 0, -1;
cal1, sal1, 0, 0, 0, 0, 0, 0, 0, cal1, sal1, 0, 0, 0, 0, 0, 0, 0;
-cal1, -sal1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, cal2, sal2, 0, 0, 0, 0, 0, 0, 0, cal2, sal2, 0, 0, 0, 0;
0, 0, 0, -cal2, -sal2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, cal3, sal3, 0, 0, 0, 0, 0, 0, 0, cal3, sal3, 0;
0, 0, 0, 0, 0, 0, -cal3, -sal3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1/3*l*3^(1/2)*cax*say*saz-1/3*l*3^(1/2)*sax*caz, -
1/3*l*3^(1/2)*sax*say*saz+1/3*l*3^(1/2)*cax*caz, 0, -1/3*l*3^(1/2)*cax*say*saz-1/3*l*3^(1/2)*sax*caz, -
1/3*l*3^(1/2)*sax*say*saz+1/3*l*3^(1/2)*cax*caz, 0, -1/3*l*3^(1/2)*cax*say*saz-1/3*l*3^(1/2)*sax*caz, -
1/3*l*3^(1/2)*sax*say*saz+1/3*l*3^(1/2)*cax*caz;
0, 0, 0, 0, 0, 0, 0, 0, 0, 1/3*l*3^(1/2)*say*saz, -1/3*l*3^(1/2)*saz*sax*cay, 1/3*l*3^(1/2)*saz*cax*cay,
1/3*l*3^(1/2)*say*saz, -1/3*l*3^(1/2)*saz*sax*cay, 1/3*l*3^(1/2)*saz*cax*cay, 1/3*l*3^(1/2)*say*saz, -
1/3*l*3^(1/2)*saz*sax*cay, 1/3*l*3^(1/2)*saz*cax*cay;
0, 0, 0, 0, 0, 0, 0, 0, 0, -1/3*l*3^(1/2)*cay*caz, -1/3*l*3^(1/2)*sax*say*caz-1/3*l*3^(1/2)*cax*saz,
1/3*l*3^(1/2)*cax*say*caz-1/3*l*3^(1/2)*sax*saz, -1/3*l*3^(1/2)*cay*caz, -1/3*l*3^(1/2)*sax*say*caz-1/3*l*3^(1/2)*cax*saz,
1/3*l*3^(1/2)*cax*say*caz-1/3*l*3^(1/2)*sax*saz, -1/3*l*3^(1/2)*cay*caz, -1/3*l*3^(1/2)*sax*say*caz-1/3*l*3^(1/2)*cax*saz,
1/3*l*3^(1/2)*cax*say*caz-1/3*l*3^(1/2)*sax*saz;
l1*sb1*sal1*sa1, -l1*sb1*cal1*sa1, l1*sb1*ca1, 0, 0, 0, 0, 0, 0, l1*sb1*sal1*sa1, -l1*sb1*cal1*sa1, l1*sb1*ca1, 0, 0, 0,
0, 0, 0;
-l1*cal1*sb1-l1*cb1*sal1*ca1, -l1*sal1*sb1+l1*cb1*cal1*ca1, l1*cb1*sa1, 0, 0, 0, 0, 0, 0, -l1*cal1*sb1-l1*cb1*sal1*ca1, -
l1*sal1*sb1+l1*cb1*cal1*ca1, l1*cb1*sa1, 0, 0, 0, 0, 0, 0;
-l2*sb2*sal1*sa2, l2*sb2*cal1*sa2, -l2*ca2*sb2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
l2*cal1*sb2+l2*cb2*sal1*ca2, l2*sal1*sb2-l2*cb2*cal1*ca2, -l2*sa2*cb2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, l3*sb3*sal2*sa3, -l3*sb3*cal2*sa3, l3*ca3*sb3, 0, 0, 0, 0, 0, 0, l3*sb3*sal2*sa3, -l3*sb3*cal2*sa3, l3*ca3*sb3,
0, 0, 0;
0, 0, 0, -l3*cal2*sb3-l3*cb3*sal2*ca3, -l3*sal2*sb3+l3*cb3*cal2*ca3, l3*sa3*cb3, 0, 0, 0, 0, 0, 0, -l3*cal2*sb3-
l3*cb3*sal2*ca3, -l3*sal2*sb3+l3*cb3*cal2*ca3, l3*sa3*cb3, 0, 0, 0;
0, 0, 0, -l4*sb4*sal2*sa4, l4*sb4*cal2*sa4, -l4*ca4*sb4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, l4*cal2*sb4+l4*cb4*sal2*ca4, l4*sal2*sb4-l4*cb4*cal2*ca4, -l4*sa4*cb4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, l5*sb5*sal3*sa5, -l5*sb5*cal3*sa5, l5*ca5*sb5, 0, 0, 0, 0, 0, 0, l5*sb5*sal3*sa5, -l5*sb5*cal3*sa5,
l5*ca5*sb5;
0, 0, 0, 0, 0, 0, -l5*cal3*sb5-l5*cb5*sal3*ca5, -l5*sal3*sb5+l5*cb5*cal3*ca5, l5*sa5*cb5, 0, 0, 0, 0, 0, 0, -l5*cal3*sb5-
l5*cb5*sal3*ca5, -l5*sal3*sb5+l5*cb5*cal3*ca5, l5*sa5*cb5;
0, 0, 0, 0, 0, 0, -l6*sb6*sal3*sa6, l6*sb6*cal3*sa6, -l6*ca6*sb6, 0, 0, 0, 0, 0, 0, 0, 0, 0;
0, 0, 0, 0, 0, 0, l6*cal3*sb6+l6*cb6*sal3*ca6, l6*sal3*sb6-l6*cb6*cal3*ca6, -l6*sa6*cb6, 0, 0, 0, 0, 0, 0, 0, 0, 0]’;
11.6.3.4. Torque Dissipation Matrix
E=[zeros(3,6), eye(6), zeros(15, 6)];
11-14 | P a g e
11.6.4. 6 joint variables + 6 task variables set
11.6.4.1. Constraint Matrix
xC=[-2*sal1*ye-2*cal1*xe+2*sal1*ya1+2*cal1*xa1+2*s1-2/3*sal1*l*3^(1/2)*sax*say*saz-
2/3*cal1*cay*saz*l*3^(1/2)+2/3*sal1*l*3^(1/2)*cax*caz,0,0,0,0,0;
0,2*cal1*xa1-2*sal1*ye+2*sal1*ya1+2*s2-2/3*sal1*l*3^(1/2)*sax*say*saz-2/3*cal1*cay*saz*l*3^(1/2)-
2*cal1*xe+2/3*sal1*l*3^(1/2)*cax*caz,0,0,0,0;
0,0,2*cal2*xa2+2*s3+1/3*cal2*cay*saz*l*3^(1/2)-sal2*l*sax*say*caz-1/3*sal2*l*3^(1/2)*cax*caz+2*sal2*ya2-2*sal2*ye-
2*cal2*xe-cal2*cay*caz*l-sal2*l*cax*saz+1/3*sal2*l*3^(1/2)*sax*say*saz,0,0,0;
0,0,0,2*cal2*xa2-2*cal2*xe+2*s4+1/3*cal2*cay*saz*l*3^(1/2)-sal2*l*sax*say*caz-1/3*sal2*l*3^(1/2)*cax*caz+2*sal2*ya2-
2*sal2*ye-cal2*cay*caz*l-sal2*l*cax*saz+1/3*sal2*l*3^(1/2)*sax*say*saz,0,0;
0,0,0,0,2*cal3*xa3+1/3*cal3*cay*saz*l*3^(1/2)+sal3*l*sax*say*caz-1/3*sal3*l*3^(1/2)*cax*caz-2*sal3*ye+2*sal3*ya3-
2*cal3*xe+2*s5+cal3*cay*caz*l+sal3*l*cax*saz+1/3*sal3*l*3^(1/2)*sax*say*saz,0;
0,0,0,0,0,-2*cal3*xe+1/3*sal3*l*3^(1/2)*sax*say*saz+2*s6+1/3*cal3*cay*saz*l*3^(1/2)+sal3*l*sax*say*caz-
1/3*sal3*l*3^(1/2)*cax*caz-2*sal3*ye+2*sal3*ya3+2*cal3*xa3+cal3*cay*caz*l+sal3*l*cax*saz;
-2*cal1*s1+2*xe-2*xa1+2/3*cay*saz*l*3^(1/2),2*xe-2*xa1+2/3*cay*saz*l*3^(1/2)-2*cal1*s2,2*xe-2*xa2-
1/3*cay*saz*l*3^(1/2)+cay*caz*l-2*cal2*s3,-2*cal2*s4+2*xe-2*xa2-1/3*cay*saz*l*3^(1/2)+cay*caz*l,2*xe-2*xa3-
1/3*cay*saz*l*3^(1/2)-cay*caz*l-2*cal3*s5,2*xe-2*xa3-1/3*cay*saz*l*3^(1/2)-cay*caz*l-2*cal3*s6;
-2*sal1*s1+2/3*l*3^(1/2)*sax*say*saz+2*ye-2*ya1-2/3*l*3^(1/2)*cax*caz,-2*sal1*s2+2/3*l*3^(1/2)*sax*say*saz+2*ye-2*ya1-
2/3*l*3^(1/2)*cax*caz,-1/3*l*3^(1/2)*sax*say*saz+2*ye-2*ya2+l*sax*say*caz+l*cax*saz-2*sal2*s3+1/3*l*3^(1/2)*cax*caz,-
2*sal2*s4-1/3*l*3^(1/2)*sax*say*saz+2*ye-2*ya2+l*sax*say*caz+l*cax*saz+1/3*l*3^(1/2)*cax*caz,-
1/3*l*3^(1/2)*sax*say*saz+2*ye-2*ya3-l*sax*say*caz-l*cax*saz-2*sal3*s5+1/3*l*3^(1/2)*cax*caz,-2*sal3*s6-
1/3*l*3^(1/2)*sax*say*saz+2*ye-2*ya3-l*sax*say*caz-l*cax*saz+1/3*l*3^(1/2)*cax*caz;
-2/3*l*3^(1/2)*cax*say*saz+2*ze-2/3*l*3^(1/2)*sax*caz,-2/3*l*3^(1/2)*cax*say*saz+2*ze-
2/3*l*3^(1/2)*sax*caz,1/3*l*3^(1/2)*cax*say*saz+2*ze-
l*cax*say*caz+l*sax*saz+1/3*l*3^(1/2)*sax*caz,1/3*l*3^(1/2)*cax*say*saz+2*ze-
l*cax*say*caz+l*sax*saz+1/3*l*3^(1/2)*sax*caz,1/3*l*3^(1/2)*cax*say*saz+2*ze+l*cax*say*caz-
l*sax*saz+1/3*l*3^(1/2)*sax*caz,1/3*l*3^(1/2)*cax*say*saz+2*ze+l*cax*say*caz-l*sax*saz+1/3*l*3^(1/2)*sax*caz;
-2/3*l*3^(1/2)*(-sax*caz*ye+cax*caz*ze-cax*say*saz*ye-
sax*say*saz*ze+cax*say*saz*ya1+sal1*s1*sax*caz+sax*caz*ya1+sal1*s1*cax*say*saz),-2/3*l*3^(1/2)*(-sax*caz*ye+cax*caz*ze-
cax*say*saz*ye-sax*say*saz*ze+cax*say*saz*ya1+sax*caz*sal1*s2+cax*say*saz*sal1*s2+sax*caz*ya1),-1/3*l*(-3*cax*say*caz*ye-
3*sax*say*caz*ze+3^(1/2)*sax*caz*ye-3^(1/2)*cax*caz*ze+3^(1/2)*cax*say*saz*ye+3^(1/2)*sax*say*saz*ze+3*sal2*s3*cax*say*caz-
sal2*s3*3^(1/2)*sax*caz-sal2*s3*3^(1/2)*cax*say*saz+3*sax*saz*ye-3*cax*saz*ze-3*sal2*s3*sax*saz+3*cax*say*caz*ya2-
ya2*3^(1/2)*sax*caz-3*sax*saz*ya2-ya2*3^(1/2)*cax*say*saz),-1/3*l*(-3*cax*say*caz*ye-3*sax*say*caz*ze+3^(1/2)*sax*caz*ye-
3^(1/2)*cax*caz*ze+3^(1/2)*cax*say*saz*ye+3^(1/2)*sax*say*saz*ze+3*sax*saz*ye-3*cax*saz*ze+3*cax*say*caz*ya2-
ya2*3^(1/2)*sax*caz-3*sax*saz*sal2*s4-sal2*s4*3^(1/2)*cax*say*saz-3*sax*saz*ya2+3*cax*say*caz*sal2*s4-
sal2*s4*3^(1/2)*sax*caz-ya2*3^(1/2)*cax*say*saz),1/3*l*(-3*cax*say*caz*ye-3*sax*say*caz*ze-
3^(1/2)*sax*caz*ye+3^(1/2)*cax*caz*ze-3^(1/2)*cax*say*saz*ye-3^(1/2)*sax*say*saz*ze+ya3*3^(1/2)*cax*say*saz+3*sax*saz*ye-
3*cax*saz*ze+3*cax*say*caz*ya3+ya3*3^(1/2)*sax*caz-3*sal3*s5*sax*saz+sal3*s5*3^(1/2)*cax*say*saz-
3*sax*saz*ya3+3*sal3*s5*cax*say*caz+sal3*s5*3^(1/2)*sax*caz),1/3*l*(-3*cax*say*caz*ye-3*sax*say*caz*ze-
3^(1/2)*sax*caz*ye+3^(1/2)*cax*caz*ze-3^(1/2)*cax*say*saz*ye-
3^(1/2)*sax*say*saz*ze+ya3*3^(1/2)*cax*say*saz+3*cax*say*caz*sal3*s6+3*sax*saz*ye-
3*cax*saz*ze+3*cax*say*caz*ya3+ya3*3^(1/2)*sax*caz-3*sax*saz*sal3*s6+sal3*s6*3^(1/2)*cax*say*saz-
3*sax*saz*ya3+sal3*s6*3^(1/2)*sax*caz);
-2/3*saz*l*3^(1/2)*(say*xe-sax*cay*ye+cax*cay*ze+sax*cay*ya1-cal1*s1*say-say*xa1+sal1*s1*sax*cay),-
2/3*saz*l*3^(1/2)*(say*xe-sax*cay*ye+cax*cay*ze+sax*cay*ya1-say*cal1*s2+sax*cay*sal1*s2-say*xa1),-1/3*l*(-
3*sax*cay*caz*ye+3*cax*cay*caz*ze-say*saz*3^(1/2)*xe+3^(1/2)*sax*cay*saz*ye-
3^(1/2)*cax*cay*saz*ze+cal2*s3*say*saz*3^(1/2)+3*sal2*s3*sax*cay*caz-sal2*s3*3^(1/2)*sax*cay*saz+3*say*caz*xe-
3*cal2*s3*say*caz+xa2*say*saz*3^(1/2)+3*ya2*sax*cay*caz-3*xa2*say*caz-ya2*3^(1/2)*sax*cay*saz),-1/3*l*(-
3*sax*cay*caz*ye+3*cax*cay*caz*ze-say*saz*3^(1/2)*xe+3^(1/2)*sax*cay*saz*ye-
3^(1/2)*cax*cay*saz*ze+3*say*caz*xe+xa2*say*saz*3^(1/2)+3*ya2*sax*cay*caz-3*cal2*s4*say*caz-sal2*s4*3^(1/2)*sax*cay*saz-
3*xa2*say*caz+cal2*s4*say*saz*3^(1/2)+3*sal2*s4*sax*cay*caz-ya2*3^(1/2)*sax*cay*saz),1/3*l*(-
3*sax*cay*caz*ye+3*cax*cay*caz*ze+say*saz*3^(1/2)*xe-
3^(1/2)*sax*cay*saz*ye+3^(1/2)*cax*cay*saz*ze+ya3*3^(1/2)*sax*cay*saz+3*say*caz*xe-xa3*say*saz*3^(1/2)+3*ya3*sax*cay*caz-
3*cal3*s5*say*caz+sal3*s5*3^(1/2)*sax*cay*saz-3*xa3*say*caz-cal3*s5*say*saz*3^(1/2)+3*sal3*s5*sax*cay*caz),1/3*l*(-
3*sax*cay*caz*ye+3*cax*cay*caz*ze+say*saz*3^(1/2)*xe-3^(1/2)*sax*cay*saz*ye+3^(1/2)*cax*cay*saz*ze+ya3*3^(1/2)*sax*cay*saz-
11-15 | 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)];